From e734413a2170a2c10a90ed9821f460e8372f2348 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sun, 1 Jun 2025 01:50:41 +0800 Subject: [PATCH] system/ui: fix path clip issues (#35409) fix path clip issues --- system/ui/onroad/model_renderer.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index ef7e567cda..a016f1cec6 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -105,13 +105,13 @@ class ModelRenderer: if model_updated: self._update_raw_points(model) - pos_x_array = self._path.raw_points[:, 0] - if pos_x_array.size == 0: + path_x_array = self._path.raw_points[:, 0] + if path_x_array.size == 0: return - self._update_model(lead_one, pos_x_array) + self._update_model(lead_one, path_x_array) if render_lead_indicator: - self._update_leads(radar_state, pos_x_array) + self._update_leads(radar_state, path_x_array) self._transform_dirty = False @@ -136,23 +136,23 @@ class ModelRenderer: self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32) self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32) - def _update_leads(self, radar_state, pos_x_array): + def _update_leads(self, radar_state, path_x_array): """Update positions of lead vehicles""" self._lead_vehicles = [LeadVehicle(), LeadVehicle()] leads = [radar_state.leadOne, radar_state.leadTwo] for i, lead_data in enumerate(leads): if lead_data and lead_data.status: d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel - idx = self._get_path_length_idx(pos_x_array, d_rel) + idx = self._get_path_length_idx(path_x_array, d_rel) z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0 point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) if point: self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect) - def _update_model(self, lead, pos_x_array): + def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" - max_distance = np.clip(pos_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) - max_idx = self._get_path_length_idx(pos_x_array, max_distance) + max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) + max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance) # Update lane lines using raw points for i, lane_line in enumerate(self._lane_lines): @@ -168,8 +168,8 @@ class ModelRenderer: if lead and lead.status: lead_d = lead.dRel * 2.0 max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) - max_idx = self._get_path_length_idx(pos_x_array, max_distance) + max_idx = self._get_path_length_idx(path_x_array, max_distance) self._path.projected_points = self._map_line_to_polygon( self._path.raw_points, 0.9, self._path_offset_z, max_idx, allow_invert=False ) @@ -309,8 +309,10 @@ class ModelRenderer: @staticmethod def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int: """Get the index corresponding to the given path height""" - idx = np.searchsorted(pos_x_array, path_height, side='right') - return int(np.clip(idx - 1, 0, len(pos_x_array) - 1)) + if len(pos_x_array) == 0: + return 0 + indices = np.where(pos_x_array <= path_height)[0] + return indices[-1] if indices.size > 0 else 0 def _map_to_screen(self, in_x, in_y, in_z): """Project a point in car space to screen space"""