UPdate

Signed-off-by: hoangtungdinh <[email protected]>

qc_opendrive/base/utils.py

@@ -103,7 +103,7 @@ def get_junction_id_map(root: etree._ElementTree) -> Dict[int, etree._ElementTre
 
 def get_left_lanes_from_lane_section(
     lane_section: etree._ElementTree,
-) -> List[etree._Element]:
+) -> List[etree._ElementTree]:
     left_lane = lane_section.find("left")
     if left_lane is not None:
         left_lanes_list = left_lane.findall("lane")
@@ -115,7 +115,7 @@ def get_left_lanes_from_lane_section(
 
 def get_right_lanes_from_lane_section(
     lane_section: etree._ElementTree,
-) -> List[etree._Element]:
+) -> List[etree._ElementTree]:
     right_lane = lane_section.find("right")
     if right_lane is not None:
         right_lanes_list = right_lane.findall("lane")
@@ -127,7 +127,7 @@ def get_right_lanes_from_lane_section(
 
 def get_left_and_right_lanes_from_lane_section(
     lane_section: etree._ElementTree,
-) -> List[etree._Element]:
+) -> List[etree._ElementTree]:
     left_lanes = get_left_lanes_from_lane_section(lane_section)
     right_lanes = get_right_lanes_from_lane_section(lane_section)
 
@@ -595,7 +595,9 @@ def get_lane_width_poly3_list(lane: etree._Element) -> List[models.OffsetPoly3]:
     return width_poly3
 
 
-def evaluate_lane_width(lane: etree._Element, ds: float) -> Union[None, float]:
+def evaluate_lane_width(
+    lane: etree._Element, s_start_from_lane_section: float
+) -> Union[None, float]:
     # This function follows the assumption that the width elements for a given
     # lane follow the standard rules for width elements.
     #
@@ -606,8 +608,8 @@ def evaluate_lane_width(lane: etree._Element, ds: float) -> Union[None, float]:
     #   polynomial function change.
     # - <width> elements shall be defined in ascending order according to the
     #   s-coordinate.
-    # - Width (ds) shall be greater than or equal to zero.
-    #
+    # - Width (s_start_from_lane_section) shall be greater than or equal to zero.
+    #   where s_start_from_lane_section = s - s_section
 
     lane_id = get_lane_id(lane)
 
@@ -621,7 +623,7 @@ def evaluate_lane_width(lane: etree._Element, ds: float) -> Union[None, float]:
 
     count = 0
     for lane_width_poly in lane_width_poly3_list:
-        if lane_width_poly.s_offset > ds:
+        if lane_width_poly.s_offset > s_start_from_lane_section:
             break
         else:
             count += 1
@@ -631,9 +633,10 @@ def evaluate_lane_width(lane: etree._Element, ds: float) -> Union[None, float]:
 
     index = count - 1
 
-    poly3_to_eval = poly3_to_polynomial(lane_width_poly3_list[index].poly3)
+    lane_width = lane_width_poly3_list[index]
+    poly3 = poly3_to_polynomial(lane_width.poly3)
 
-    return poly3_to_eval(ds)
+    return poly3(s_start_from_lane_section - lane_width.s_offset)
 
 
 def get_connections_between_road_and_junction(
@@ -1555,11 +1558,131 @@ def get_lane_section_from_road_by_s(
     return lane_section_list[lane_section_index]
 
 
+def get_lane_offset_from_road_by_s(
+    road: etree._ElementTree, s: float
+) -> Union[None, models.OffsetPoly3]:
+    length = get_road_length(road)
+
+    if s < 0.0 or s > length:
+        return None
+
+    lane_offset_list = get_lane_offsets_from_road(road)
+
+    # Default lane offset is zero.
+    if len(lane_offset_list) == 0:
+        return ZERO_OFFSET_POLY3
+
+    lane_offset_indexes = [l.s_offset for l in lane_offset_list]
+    lane_offset_index = np.searchsorted(lane_offset_indexes, s, side="right") - 1
+
+    if lane_offset_index < 0:
+        # It's possible that s_offset does not start from zero.
+        # In this case, lane offset is zero for s < first s_offset.
+        return ZERO_OFFSET_POLY3
+    else:
+        return lane_offset_list[lane_offset_index]
+
+
+def get_lane_offset_value_from_road_by_s(
+    road: etree._ElementTree, s: float
+) -> Union[None, float]:
+    lane_offset = get_lane_offset_from_road_by_s(road, s)
+
+    if lane_offset is None:
+        return None
+
+    poly3 = poly3_to_polynomial(lane_offset.poly3)
+
+    return poly3(s - lane_offset.s_offset)
+
+
+def evaluate_lane_border(
+    lane: etree._Element, s_start_from_lane_section: float
+) -> Union[None, float]:
+    """
+    s_start_from_lane_section shall be greater than or equal to zero.
+    s_start_from_lane_section = s - s_section
+    """
+
+    lane_id = get_lane_id(lane)
+
+    if lane_id == 0:
+        return 0.0
+
+    lane_border_poly3_list = get_borders_from_lane(lane)
+
+    if len(lane_border_poly3_list) == 0:
+        return None
+
+    count = 0
+    for lane_border_poly in lane_border_poly3_list:
+        if lane_border_poly.s_offset > s_start_from_lane_section:
+            break
+        else:
+            count += 1
+
+    if count == 0:
+        return None
+
+    index = count - 1
+
+    lane_border = lane_border_poly3_list[index]
+
+    poly3 = poly3_to_polynomial(lane_border.poly3)
+
+    return poly3(s_start_from_lane_section - lane_border.s_offset)
+
+
+def get_border_points_from_lane_group_by_s(
+    lane_group: List[etree._ElementTree], lane_offset: float, s_section: float, s: float
+) -> Dict[int, float]:
+    """
+    Returns a dictionary where key is the lane id and value is the border point t value
+    """
+    id_to_width = dict()
+    id_to_border_point_t = dict()
+
+    for lane in lane_group:
+        lane_id = get_lane_id(lane)
+        width = evaluate_lane_width(lane, s - s_section)
+        if width is None:
+            border_point_t = evaluate_lane_border(lane, s - s_section)
+            id_to_border_point_t[lane_id] = border_point_t
+        else:
+            id_to_width[lane_id] = width
+
+
+def get_lane_from_lane_section_by_st(
+    lane_section: etree._ElementTree, lane_offset: float, s: float, t: float
+) -> Union[None, etree._ElementTree]:
+    s_section = get_s_from_lane_section(lane_section)
+
+    left_lane_group = get_left_lanes_from_lane_section(lane_section)
+    right_lane_group = get_right_lanes_from_lane_section(lane_section)
+
+    left_border_points = get_border_points_from_lane_group_by_s(
+        left_lane_group, lane_offset, s_section, s
+    )
+
+    right_border_points = get_border_points_from_lane_group_by_s(
+        right_lane_group, lane_offset, s_section, s
+    )
+
+
 def get_surface_point_xyz_from_road(
     road: etree._ElementTree, s: float, t: float
 ) -> Union[None, models.Point3D]:
     lane_section = get_lane_section_from_road_by_s(road, s)
-    lane = get_lane_from_lane_section_by_t(lane_section, t)
+
+    if lane_section is None:
+        return None
+
+    lane_offset = get_lane_offset_value_from_road_by_s(road, s)
+
+    lane = get_lane_from_lane_section_by_st(lane_section, lane_offset, s, t)
+
+    if lane is None:
+        return None
 
     t_inner_border = get_t_inner_border_from_lane(lane, s)