From bad8cf34b391bbb79faff277ec797049258e1007 Mon Sep 17 00:00:00 2001 From: thftgr Date: Sat, 1 Aug 2026 01:19:51 +0900 Subject: [PATCH 1/2] Optimize road-camera CPU usage by implementing batching. Revamped road-camera overlay rendering to batch contiguous visible segments and reuse projection buffers, reducing redundant computations. Achieved a notable FPS and CPU performance improvement while preserving UI fidelity and maintaining exact legacy fallback functionality. Extensive profiling, regression tests, and visual validation were conducted to ensure correctness and stability. --- .../carrot/cluster/cluster_renderer.py | 166 +++++++++++++---- .../carrot/tests/test_cluster_navi.py | 2 +- .../carrot/tests/test_cluster_scene.py | 176 ++++++++++++++++-- 3 files changed, 286 insertions(+), 58 deletions(-) diff --git a/openpilot/selfdrive/carrot/cluster/cluster_renderer.py b/openpilot/selfdrive/carrot/cluster/cluster_renderer.py index fd7a0daa90..256d139ec4 100644 --- a/openpilot/selfdrive/carrot/cluster/cluster_renderer.py +++ b/openpilot/selfdrive/carrot/cluster/cluster_renderer.py @@ -929,6 +929,10 @@ def __init__( tuple[int, int], tuple[tuple[Vec3, ...], tuple[Vec3, ...], object, int], ] = OrderedDict() + self._camera_overlay_strip_points = None + self._camera_overlay_strip_point_capacity = 0 + self._camera_overlay_strip_pair_visibility = bytearray() + self._raw_draw_triangle_strip_2d = getattr(getattr(rl, "rl", None), "DrawTriangleStrip", None) self._world_label_texture_cache: OrderedDict[ tuple[int, str, float, float, tuple[int, int, int, int]], CachedTextTexture, @@ -1273,6 +1277,9 @@ def close(self) -> None: self._vehicle_model_load_attempted = False self._scene_cache_key = None self._scene_cache = None + self._camera_overlay_strip_points = None + self._camera_overlay_strip_point_capacity = 0 + self._camera_overlay_strip_pair_visibility = bytearray() self._route_video_size = None self._route_video_frame_id = None rl.close_window() @@ -1448,11 +1455,12 @@ def _render_world(self, state: ClusterUiState, signal_lights: tuple[bool, bool] rl.clear_background(rl_color(theme.bg)) self._profile_add("render_world.clear_background", profile_stage) if state.camera_view_mode == CLUSTER_CAMERA_VIEW_MODE_ROAD_CAMERA: + projection = self._camera_overlay_projection(state) profile_stage = self._profile_start() - self._draw_camera_background(state) + self._draw_camera_background(state, projection) self._profile_add("render_world.camera_background", profile_stage) profile_stage = self._profile_start() - self._draw_camera_projected_overlay(scene, state) + self._draw_camera_projected_overlay(scene, state, projection) self._profile_add("render_world.camera_projected_overlay", profile_stage) else: profile_stage = self._profile_start() @@ -1478,11 +1486,16 @@ def _scene_for_state( self._scene_cache = scene return scene - def _draw_camera_background(self, state: ClusterUiState) -> None: + def _draw_camera_background( + self, + state: ClusterUiState, + projection: CameraOverlayProjection | None = None, + ) -> None: if state.camera_view_mode != CLUSTER_CAMERA_VIEW_MODE_ROAD_CAMERA: return overlay = state.route_overlay - projection = self._camera_overlay_projection(state) + if projection is None: + projection = self._camera_overlay_projection(state) if projection is None: return texture = None @@ -1625,12 +1638,12 @@ def _camera_overlay_projection(self, state: ClusterUiState) -> CameraOverlayProj camera_height_m=camera_height_m, ) - def _project_camera_overlay_point( + def _camera_overlay_screen_xy( self, point: Vec3, projection: CameraOverlayProjection, scene_shift_x_m: float = 0.0, - ) -> rl.Vector2 | None: + ) -> tuple[float, float] | None: # Scene geometry is shifted forward for the synthetic 3D camera. Camera # projection uses the physical road coordinate whose origin is the ego. road_forward = float(point.y) - EGO_FORWARD_M @@ -1639,16 +1652,18 @@ def _project_camera_overlay_point( if road_forward <= CAMERA_OVERLAY_MIN_DEPTH_M: return None - matrix = projection.view_from_road - view_x = matrix[0][0] * road_forward + matrix[0][1] * road_left + matrix[0][2] * road_up + row_x, row_y, row_z = projection.view_from_road + view_x = row_x[0] * road_forward + row_x[1] * road_left + row_x[2] * road_up view_y = ( - matrix[1][0] * road_forward - + matrix[1][1] * road_left - + matrix[1][2] * road_up + row_y[0] * road_forward + + row_y[1] * road_left + + row_y[2] * road_up + projection.camera_height_m ) - view_z = matrix[2][0] * road_forward + matrix[2][1] * road_left + matrix[2][2] * road_up - if view_z <= CAMERA_OVERLAY_MIN_DEPTH_M or not all(math.isfinite(value) for value in (view_x, view_y, view_z)): + view_z = row_z[0] * road_forward + row_z[1] * road_left + row_z[2] * road_up + if view_z <= CAMERA_OVERLAY_MIN_DEPTH_M or not ( + math.isfinite(view_x) and math.isfinite(view_y) and math.isfinite(view_z) + ): return None camera_x = projection.focal_length * view_x / view_z + projection.camera_width * 0.5 @@ -1663,10 +1678,16 @@ def _project_camera_overlay_point( or screen_y > projection.dest.y + projection.dest.height + clip_margin ): return None - return rl.Vector2(screen_x, screen_y) + return screen_x, screen_y - def _draw_camera_projected_overlay(self, scene: ClusterScene, state: ClusterUiState) -> None: - projection = self._camera_overlay_projection(state) + def _draw_camera_projected_overlay( + self, + scene: ClusterScene, + state: ClusterUiState, + projection: CameraOverlayProjection | None = None, + ) -> None: + if projection is None: + projection = self._camera_overlay_projection(state) if projection is None: return @@ -1677,6 +1698,7 @@ def _draw_camera_projected_overlay(self, scene: ClusterScene, state: ClusterUiSt int(round(projection.dest.height)), ) try: + profile_stage = self._profile_start() for strip in scene.highlight_lanes: self._draw_camera_overlay_strip(strip, projection, scene.scene_shift_x_m) for strip in scene.road_edges: @@ -1685,10 +1707,15 @@ def _draw_camera_projected_overlay(self, scene: ClusterScene, state: ClusterUiSt self._draw_camera_overlay_strip(strip, projection, scene.scene_shift_x_m) for strip in scene.planned_path: self._draw_camera_overlay_strip(strip, projection, scene.scene_shift_x_m) + self._profile_add("render_world.camera_projected_overlay.strips", profile_stage) + profile_stage = self._profile_start() for point in scene.radar_points: self._draw_camera_overlay_radar_point(point, projection, scene.scene_shift_x_m, state.radar_info_mode) + self._profile_add("render_world.camera_projected_overlay.radar", profile_stage) + profile_stage = self._profile_start() for vehicle in sorted(scene.vehicles, key=self._camera_overlay_vehicle_draw_key): self._draw_camera_overlay_vehicle(vehicle, projection, scene.scene_shift_x_m, state.radar_info_mode) + self._profile_add("render_world.camera_projected_overlay.vehicles", profile_stage) finally: rl.end_scissor_mode() @@ -1702,16 +1729,75 @@ def _draw_camera_overlay_strip( if count < 2: return color = rl_color(strip.color) + points = self._camera_overlay_strip_point_buffer(count * 2) + pair_visible = self._camera_overlay_strip_visibility_buffer(count) + x_offset_m = scene_shift_x_m + strip.x_offset_m + project_point = self._camera_overlay_screen_xy + left_points = strip.left + right_points = strip.right + for index in range(count): + pair_visible[index] = 0 + left = project_point(left_points[index], projection, x_offset_m) + right = project_point(right_points[index], projection, x_offset_m) + if left is None or right is None: + continue + left_point = points[index * 2] + left_point.x, left_point.y = left + right_point = points[index * 2 + 1] + right_point.x, right_point.y = right + pair_visible[index] = 1 + + draw_triangle_strip = getattr(self, "_raw_draw_triangle_strip_2d", None) + if draw_triangle_strip is not None: + point_ptr = rl.ffi.cast("struct Vector2 *", points) + index = 0 + while index < count: + # Split at rejected endpoint pairs so batching cannot bridge a + # gap that the legacy per-segment clipping left empty. + while index < count and not pair_visible[index]: + index += 1 + start = index + while index < count and pair_visible[index]: + index += 1 + pair_count = index - start + if pair_count >= 2: + # L0,R0,L1,R1 expands to the same two triangles and + # winding as the legacy DrawTriangle pair below. + draw_triangle_strip(point_ptr + start * 2, pair_count * 2, color) + return + for index in range(count - 1): - left0 = self._project_camera_overlay_point(strip.left[index], projection, scene_shift_x_m + strip.x_offset_m) - right0 = self._project_camera_overlay_point(strip.right[index], projection, scene_shift_x_m + strip.x_offset_m) - left1 = self._project_camera_overlay_point(strip.left[index + 1], projection, scene_shift_x_m + strip.x_offset_m) - right1 = self._project_camera_overlay_point(strip.right[index + 1], projection, scene_shift_x_m + strip.x_offset_m) - if left0 is None or right0 is None or left1 is None or right1 is None: + if not pair_visible[index] or not pair_visible[index + 1]: continue + left0 = points[index * 2] + right0 = points[index * 2 + 1] + left1 = points[(index + 1) * 2] + right1 = points[(index + 1) * 2 + 1] rl.draw_triangle(left0, right0, right1, color) rl.draw_triangle(left0, right1, left1, color) + def _camera_overlay_strip_point_buffer(self, point_count: int): + capacity = getattr(self, "_camera_overlay_strip_point_capacity", 0) + points = getattr(self, "_camera_overlay_strip_points", None) + if points is not None and capacity >= point_count: + return points + + capacity = 1 << max(0, point_count - 1).bit_length() + points = rl.ffi.new("struct Vector2[]", capacity) + self._camera_overlay_strip_points = points + self._camera_overlay_strip_point_capacity = capacity + return points + + def _camera_overlay_strip_visibility_buffer(self, pair_count: int) -> bytearray: + pair_visible = getattr(self, "_camera_overlay_strip_pair_visibility", None) + if pair_visible is None: + pair_visible = bytearray() + if len(pair_visible) < pair_count: + capacity = 1 << max(0, pair_count - 1).bit_length() + pair_visible.extend(bytes(capacity - len(pair_visible))) + self._camera_overlay_strip_pair_visibility = pair_visible + return pair_visible + def _draw_camera_overlay_vehicle( self, vehicle: VehicleBox, @@ -1743,7 +1829,7 @@ def _draw_camera_overlay_vehicle_frame( ) -> None: center_y_m = vehicle.center.y + RADAR_TO_CAMERA_M + VEHICLE_LENGTH_M base_z = CAMERA_OVERLAY_VEHICLE_ROAD_HEIGHT_M - center = self._project_camera_overlay_point( + center = self._camera_overlay_screen_xy( Vec3(vehicle.center.x, center_y_m, base_z), projection, scene_shift_x_m, @@ -1757,7 +1843,7 @@ def _draw_camera_overlay_vehicle_frame( emphasized = vehicle.primary or vehicle.cut_in half_width_m = max(0.6, vehicle.width_m * 0.58) frame_height_m = max(0.8, vehicle.height_m * 1.12) - left_base = self._project_camera_overlay_point( + left_base = self._camera_overlay_screen_xy( Vec3( vehicle.center.x - half_width_m, center_y_m, @@ -1766,7 +1852,7 @@ def _draw_camera_overlay_vehicle_frame( projection, scene_shift_x_m, ) - right_base = self._project_camera_overlay_point( + right_base = self._camera_overlay_screen_xy( Vec3( vehicle.center.x + half_width_m, center_y_m, @@ -1775,7 +1861,7 @@ def _draw_camera_overlay_vehicle_frame( projection, scene_shift_x_m, ) - left_top = self._project_camera_overlay_point( + left_top = self._camera_overlay_screen_xy( Vec3( vehicle.center.x - half_width_m, center_y_m, @@ -1784,7 +1870,7 @@ def _draw_camera_overlay_vehicle_frame( projection, scene_shift_x_m, ) - right_top = self._project_camera_overlay_point( + right_top = self._camera_overlay_screen_xy( Vec3( vehicle.center.x + half_width_m, center_y_m, @@ -1793,17 +1879,16 @@ def _draw_camera_overlay_vehicle_frame( projection, scene_shift_x_m, ) - projected_corners = (left_base, right_base, left_top, right_top) - if any(point is None for point in projected_corners): + if left_base is None or right_base is None or left_top is None or right_top is None: return - min_x = min(point.x for point in projected_corners if point is not None) - max_x = max(point.x for point in projected_corners if point is not None) - min_y = min(point.y for point in projected_corners if point is not None) - max_y = max(point.y for point in projected_corners if point is not None) + min_x = min(left_base[0], right_base[0], left_top[0], right_top[0]) + max_x = max(left_base[0], right_base[0], left_top[0], right_top[0]) + min_y = min(left_base[1], right_base[1], left_top[1], right_top[1]) + max_y = max(left_base[1], right_base[1], left_top[1], right_top[1]) width = max_x - min_x height = max_y - min_y - if not all(math.isfinite(value) for value in (width, height)) or width <= 0.0 or height <= 0.0: + if not (math.isfinite(width) and math.isfinite(height)) or width <= 0.0 or height <= 0.0: return pad_x = max(4.0, width * 0.08) pad_y = max(4.0, height * 0.08) @@ -1892,13 +1977,13 @@ def _draw_camera_overlay_radar_point( radar_info_mode: int, ) -> None: camera_point = Vec3(point.center.x, point.center.y + RADAR_TO_CAMERA_M, point.center.z) - screen = self._project_camera_overlay_point(camera_point, projection, scene_shift_x_m) + screen = self._camera_overlay_screen_xy(camera_point, projection, scene_shift_x_m) if screen is None: return radius = max(3.0, min(10.0, 80.0 / max(6.0, point.longitudinal_m))) marker = rl.Rectangle( - screen.x - radius, - screen.y - radius, + screen[0] - radius, + screen[1] - radius, radius * 2.0, radius * 2.0, ) @@ -1917,7 +2002,14 @@ def _draw_camera_overlay_radar_point( if point.absolute_speed_kph is not None: label_parts.append(f"{display_speed(point.absolute_speed_kph, self.is_metric):.0f} {speed_unit(self.is_metric)}") if label_parts: - self._draw_world_label_text(" ".join(label_parts), screen.x, screen.y - 16.0, 15, (*WHITE[:3], 220), anchor="center") + self._draw_world_label_text( + " ".join(label_parts), + screen[0], + screen[1] - 16.0, + 15, + (*WHITE[:3], 220), + anchor="center", + ) def render_to_file(self, state: ClusterUiState, output_path: str | Path) -> None: image = self._render_to_image(state) diff --git a/openpilot/selfdrive/carrot/tests/test_cluster_navi.py b/openpilot/selfdrive/carrot/tests/test_cluster_navi.py index 1fe56cc378..597e25294a 100644 --- a/openpilot/selfdrive/carrot/tests/test_cluster_navi.py +++ b/openpilot/selfdrive/carrot/tests/test_cluster_navi.py @@ -862,7 +862,7 @@ def test_road_camera_vehicle_frame_uses_vehicle_road_anchor(monkeypatch): projected = [] monkeypatch.setattr( ClusterUiRenderer, - "_project_camera_overlay_point", + "_camera_overlay_screen_xy", lambda self, point, projection, scene_shift_x_m=0.0: (projected.append(point), None)[1], ) renderer._draw_camera_overlay_vehicle_frame( diff --git a/openpilot/selfdrive/carrot/tests/test_cluster_scene.py b/openpilot/selfdrive/carrot/tests/test_cluster_scene.py index 1fe57816cd..f4c7c893c5 100644 --- a/openpilot/selfdrive/carrot/tests/test_cluster_scene.py +++ b/openpilot/selfdrive/carrot/tests/test_cluster_scene.py @@ -23,6 +23,7 @@ from cluster_renderer import ClusterUiRenderer import cluster_scene from cluster_scene import ( + MeshStrip, RadarPointMarker, SCENE_STATE_FIELDS, Vec3, @@ -47,7 +48,7 @@ def test_road_camera_radar_point_uses_transparent_source_colored_frame(monkeypat lateral_m=1.0, ) - monkeypatch.setattr(renderer, "_project_camera_overlay_point", lambda *_args: cluster_renderer.rl.Vector2(100.0, 80.0)) + monkeypatch.setattr(renderer, "_camera_overlay_screen_xy", lambda *_args: (100.0, 80.0)) monkeypatch.setattr( cluster_renderer.rl, "draw_rectangle_rounded_lines_ex", @@ -67,11 +68,11 @@ def test_road_camera_detected_vehicle_uses_transparent_colored_rounded_frame(mon renderer = object.__new__(ClusterUiRenderer) outlines = [] projected = iter(( - cluster_renderer.rl.Vector2(100.0, 100.0), - cluster_renderer.rl.Vector2(80.0, 100.0), - cluster_renderer.rl.Vector2(120.0, 100.0), - cluster_renderer.rl.Vector2(80.0, 50.0), - cluster_renderer.rl.Vector2(120.0, 50.0), + (100.0, 100.0), + (80.0, 100.0), + (120.0, 100.0), + (80.0, 50.0), + (120.0, 50.0), )) vehicle = VehicleBox( center=Vec3(0.0, 25.0, 0.0), @@ -91,7 +92,7 @@ def test_road_camera_detected_vehicle_uses_transparent_colored_rounded_frame(mon longitudinal_m=25.0, ) - monkeypatch.setattr(renderer, "_project_camera_overlay_point", lambda *_args: next(projected)) + monkeypatch.setattr(renderer, "_camera_overlay_screen_xy", lambda *_args: next(projected)) monkeypatch.setattr( cluster_renderer.rl, "draw_rectangle_rounded_lines_ex", @@ -144,23 +145,23 @@ def test_road_camera_vehicle_frame_rejects_incomplete_or_edge_clipped_projection ) incomplete = iter(( - cluster_renderer.rl.Vector2(150.0, 100.0), - cluster_renderer.rl.Vector2(130.0, 100.0), + (150.0, 100.0), + (130.0, 100.0), None, - cluster_renderer.rl.Vector2(130.0, 50.0), - cluster_renderer.rl.Vector2(170.0, 50.0), + (130.0, 50.0), + (170.0, 50.0), )) - monkeypatch.setattr(renderer, "_project_camera_overlay_point", lambda *_args: next(incomplete)) + monkeypatch.setattr(renderer, "_camera_overlay_screen_xy", lambda *_args: next(incomplete)) renderer._draw_camera_overlay_vehicle_frame(vehicle, projection, 0.0, CLUSTER_RADAR_INFO_NONE) edge_clipped = iter(( - cluster_renderer.rl.Vector2(5.0, 100.0), - cluster_renderer.rl.Vector2(-15.0, 100.0), - cluster_renderer.rl.Vector2(25.0, 100.0), - cluster_renderer.rl.Vector2(-15.0, 50.0), - cluster_renderer.rl.Vector2(25.0, 50.0), + (5.0, 100.0), + (-15.0, 100.0), + (25.0, 100.0), + (-15.0, 50.0), + (25.0, 50.0), )) - monkeypatch.setattr(renderer, "_project_camera_overlay_point", lambda *_args: next(edge_clipped)) + monkeypatch.setattr(renderer, "_camera_overlay_screen_xy", lambda *_args: next(edge_clipped)) renderer._draw_camera_overlay_vehicle_frame(vehicle, projection, 0.0, CLUSTER_RADAR_INFO_NONE) assert outlines == [] @@ -189,9 +190,9 @@ def test_road_camera_vehicle_frame_ignores_radar_yaw_for_screen_box_width(monkey def project(_self, point, _projection, _scene_shift_x_m=0.0): projected_road_points.append(point) - return cluster_renderer.rl.Vector2(150.0 + point.x * 10.0, 100.0 - point.z * 20.0) + return 150.0 + point.x * 10.0, 100.0 - point.z * 20.0 - monkeypatch.setattr(ClusterUiRenderer, "_project_camera_overlay_point", project) + monkeypatch.setattr(ClusterUiRenderer, "_camera_overlay_screen_xy", project) monkeypatch.setattr(cluster_renderer.rl, "draw_rectangle_rounded_lines_ex", lambda *_args: None) projection = SimpleNamespace(dest=cluster_renderer.rl.Rectangle(0.0, 0.0, 300.0, 200.0)) renderer._draw_camera_overlay_vehicle_frame(vehicle, projection, 0.0, CLUSTER_RADAR_INFO_NONE) @@ -201,6 +202,141 @@ def project(_self, point, _projection, _scene_shift_x_m=0.0): assert left_base.x < vehicle.center.x < right_base.x +def test_road_camera_strip_projects_each_endpoint_once_and_reuses_buffers(monkeypatch): + renderer = object.__new__(ClusterUiRenderer) + renderer._camera_overlay_strip_points = None + renderer._camera_overlay_strip_point_capacity = 0 + renderer._camera_overlay_strip_pair_visibility = bytearray() + projected = [] + batches = [] + strip = MeshStrip( + left=(Vec3(-1.0, 10.0), Vec3(-2.0, 20.0), Vec3(-3.0, 30.0)), + right=(Vec3(1.0, 10.0), Vec3(2.0, 20.0), Vec3(3.0, 30.0)), + color=(10, 20, 30, 40), + x_offset_m=0.25, + ) + + def project(point, _projection, scene_shift_x_m=0.0): + projected.append((point, scene_shift_x_m)) + return point.x * 10.0, point.y + + def draw_strip(points, point_count, _color): + batches.append(tuple((points[index].x, points[index].y) for index in range(point_count))) + + monkeypatch.setattr(renderer, "_camera_overlay_screen_xy", project) + renderer._raw_draw_triangle_strip_2d = draw_strip + + renderer._draw_camera_overlay_strip(strip, object(), 0.5) + point_buffer = renderer._camera_overlay_strip_points + visibility_buffer = renderer._camera_overlay_strip_pair_visibility + renderer._draw_camera_overlay_strip(strip, object(), 0.5) + + expected_batch = ( + (-10.0, 10.0), (10.0, 10.0), + (-20.0, 20.0), (20.0, 20.0), + (-30.0, 30.0), (30.0, 30.0), + ) + assert batches == [expected_batch, expected_batch] + assert len(projected) == 12 + assert all(offset == pytest.approx(0.75) for _point, offset in projected) + assert renderer._camera_overlay_strip_points is point_buffer + assert renderer._camera_overlay_strip_pair_visibility is visibility_buffer + + +def test_road_camera_strip_batches_only_contiguous_visible_pairs(monkeypatch): + renderer = object.__new__(ClusterUiRenderer) + renderer._camera_overlay_strip_points = None + renderer._camera_overlay_strip_point_capacity = 0 + renderer._camera_overlay_strip_pair_visibility = bytearray() + batches = [] + strip = MeshStrip( + left=tuple(Vec3(-1.0, float(index)) for index in range(6)), + right=tuple(Vec3(1.0, float(index)) for index in range(6)), + color=(255, 255, 255, 255), + ) + + def project(point, _projection, _scene_shift_x_m=0.0): + if point.y == 2.0: + return None + return point.x, point.y + + def draw_strip(points, point_count, _color): + batches.append(tuple((points[index].x, points[index].y) for index in range(point_count))) + + monkeypatch.setattr(renderer, "_camera_overlay_screen_xy", project) + renderer._raw_draw_triangle_strip_2d = draw_strip + renderer._draw_camera_overlay_strip(strip, object(), 0.0) + + assert batches == [ + ((-1.0, 0.0), (1.0, 0.0), (-1.0, 1.0), (1.0, 1.0)), + ((-1.0, 3.0), (1.0, 3.0), (-1.0, 4.0), (1.0, 4.0), (-1.0, 5.0), (1.0, 5.0)), + ] + + +def test_road_camera_strip_fallback_preserves_original_triangle_order(monkeypatch): + renderer = object.__new__(ClusterUiRenderer) + renderer._camera_overlay_strip_points = None + renderer._camera_overlay_strip_point_capacity = 0 + renderer._camera_overlay_strip_pair_visibility = bytearray() + renderer._raw_draw_triangle_strip_2d = None + triangles = [] + strip = MeshStrip( + left=(Vec3(-1.0, 10.0), Vec3(-2.0, 20.0), Vec3(-3.0, 30.0)), + right=(Vec3(1.0, 10.0), Vec3(2.0, 20.0), Vec3(3.0, 30.0)), + color=(10, 20, 30, 40), + ) + + monkeypatch.setattr( + renderer, + "_camera_overlay_screen_xy", + lambda point, _projection, _scene_shift_x_m=0.0: (point.x, point.y), + ) + monkeypatch.setattr( + cluster_renderer.rl, + "draw_triangle", + lambda p0, p1, p2, _color: triangles.append(((p0.x, p0.y), (p1.x, p1.y), (p2.x, p2.y))), + ) + + renderer._draw_camera_overlay_strip(strip, object(), 0.0) + + assert triangles == [ + ((-1.0, 10.0), (1.0, 10.0), (2.0, 20.0)), + ((-1.0, 10.0), (2.0, 20.0), (-2.0, 20.0)), + ((-2.0, 20.0), (2.0, 20.0), (3.0, 30.0)), + ((-2.0, 20.0), (3.0, 30.0), (-3.0, 30.0)), + ] + + +def test_road_camera_world_reuses_one_projection_for_background_and_overlay(monkeypatch): + renderer = object.__new__(ClusterUiRenderer) + renderer.profile_enabled = False + state = SimpleNamespace(camera_view_mode=CLUSTER_CAMERA_VIEW_MODE_ROAD_CAMERA, onroad=True) + theme = SimpleNamespace(bg=(1, 2, 3, 255)) + scene = object() + projection = object() + projection_calls = [] + background_calls = [] + overlay_calls = [] + + monkeypatch.setattr(renderer, "_current_theme", lambda: theme) + monkeypatch.setattr(renderer, "_highlight_lane_lit", lambda *_args: False) + monkeypatch.setattr(renderer, "_scene_for_state", lambda *_args: scene) + monkeypatch.setattr( + renderer, + "_camera_overlay_projection", + lambda value: projection_calls.append(value) or projection, + ) + monkeypatch.setattr(renderer, "_draw_camera_background", lambda *args: background_calls.append(args)) + monkeypatch.setattr(renderer, "_draw_camera_projected_overlay", lambda *args: overlay_calls.append(args)) + monkeypatch.setattr(cluster_renderer.rl, "clear_background", lambda _color: None) + + renderer._render_world(state, (False, False)) + + assert projection_calls == [state] + assert background_calls == [(state, projection)] + assert overlay_calls == [(scene, state, projection)] + + def _cluster_state(**changes) -> ClusterUiState: state = ClusterUiState( speed_kph=60.0, From a6ed39be894e4f44d58de5eb2eab464c0c8c3641 Mon Sep 17 00:00:00 2001 From: thftgr Date: Sat, 1 Aug 2026 01:33:41 +0900 Subject: [PATCH 2/2] Remove synthetic BSD-based virtual vehicles in cluster logic Eliminates the creation of fixed-position rear virtual vehicles based solely on BSD signals without valid distance fields. Preserves behavior for measured corner vehicles and ensures BSD lane highlights remain functional. Updated tests verify compliance with the new logic. --- .../carrot/cluster/cluster_route_replay.py | 24 ------------------ .../carrot/tests/test_cluster_route_replay.py | 25 +++++++++++++++++++ 2 files changed, 25 insertions(+), 24 deletions(-) diff --git a/openpilot/selfdrive/carrot/cluster/cluster_route_replay.py b/openpilot/selfdrive/carrot/cluster/cluster_route_replay.py index 5c1a491720..ad6ce3cec2 100644 --- a/openpilot/selfdrive/carrot/cluster/cluster_route_replay.py +++ b/openpilot/selfdrive/carrot/cluster/cluster_route_replay.py @@ -204,8 +204,6 @@ class DbcSignalSpec: NAV_SPEED_LIMIT_HOLD_SECONDS = 10.0 ROAD_EDGE_VEHICLE_OUTSIDE_MARGIN_M = 0.25 NEAR_ROAD_EDGE_VEHICLE_BLOCK_DISTANCE_M = 10.0 -BSD_SUMMARY_LONGITUDINAL_M = -1.6 -BSD_SUMMARY_LATERAL_M = 2.9 LANE_CHANGE_REINDEX_PEAK_THRESHOLD = 0.22 LANE_CHANGE_REINDEX_RESET_THRESHOLD = -0.08 CONTINUOUS_LANE_CHANGE_REBASE_PROGRESS = 0.12 @@ -4481,31 +4479,9 @@ def car_state_corner_detections(car_state: Any) -> tuple[DetectedVehicle, ...]: source="carState", ) ) - if left_blindspot and not car_state_has_near_side_detection(detections, -1.0): - detections.append(blindspot_summary_detection("LR", -1.0)) - if right_blindspot and not car_state_has_near_side_detection(detections, 1.0): - detections.append(blindspot_summary_detection("RR", 1.0)) return tuple(detections) -def car_state_has_near_side_detection(detections: list[DetectedVehicle], side: float) -> bool: - return any( - math.copysign(1.0, vehicle.lateral_m) == side - and -8.0 <= vehicle.longitudinal_m <= 8.0 - for vehicle in detections - ) - - -def blindspot_summary_detection(label: str, side: float) -> DetectedVehicle: - return DetectedVehicle( - label=label, - longitudinal_m=BSD_SUMMARY_LONGITUDINAL_M, - lateral_m=side * BSD_SUMMARY_LATERAL_M, - source="carState", - probability=0.85, - ) - - def parse_corner_radar_message( address: int, data: bytes, diff --git a/openpilot/selfdrive/carrot/tests/test_cluster_route_replay.py b/openpilot/selfdrive/carrot/tests/test_cluster_route_replay.py index 6fde41f12e..dfd5a405bf 100644 --- a/openpilot/selfdrive/carrot/tests/test_cluster_route_replay.py +++ b/openpilot/selfdrive/carrot/tests/test_cluster_route_replay.py @@ -15,6 +15,7 @@ StableCornerObjectTracker, adjacent_route_log_path, blend_frames, + car_state_corner_detections, corner_track_label, frame_to_state, model_lead_detections_from_model_v2, @@ -50,6 +51,30 @@ def radar_lead(track_id, d_rel=25.0, y_rel=2.0): ) +def test_blindspot_flags_do_not_create_virtual_corner_vehicles(): + detections = car_state_corner_detections(SimpleNamespace( + leftBlindspot=True, + rightBlindspot=True, + )) + + assert detections == () + + +def test_blindspot_rear_distance_still_creates_measured_corner_vehicle(): + detections = car_state_corner_detections(SimpleNamespace( + leftBlindspot=True, + rightBlindspot=False, + leftRearLongDist=3.2, + leftRearLatDist=2.4, + )) + + assert len(detections) == 1 + assert detections[0].label == "LR" + assert detections[0].longitudinal_m == -3.2 + assert detections[0].lateral_m == -2.4 + assert detections[0].source == "carState" + + def test_corner_430_track_labels_preserve_radar_group(): assert corner_track_label(300, "corner430") == "CR430_000" assert corner_track_label(411, "corner430") == "CR430_111"