system/ui: fix path clip issues (#35409)

fix path clip issues

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"""