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

fix path clip issues
pull/35410/head
Dean Lee 3 months ago committed by GitHub
parent e9f917836b
commit e734413a21
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
  1. 26
      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"""

Loading…
Cancel
Save