diff --git a/qc_opendrive/base/utils.py b/qc_opendrive/base/utils.py index c19ea5c..2d43061 100644 --- a/qc_opendrive/base/utils.py +++ b/qc_opendrive/base/utils.py @@ -1517,22 +1517,22 @@ def get_point_xyz_from_road( road: etree._ElementTree, s: float, t: float, h: float ) -> Union[None, models.Point3D]: yaw = get_heading_from_road_reference_line(road, s) - # A reference line doesn't have pitch. - pitch = 0.0 roll = get_roll_from_road_reference_line(road, s) - if yaw is None or pitch is None or roll is None: + if yaw is None or roll is None: return None - rotation = transforms3d.euler.euler2mat(yaw, pitch, roll, "rzyx") + rotation = transforms3d.euler.euler2mat(yaw, 0.0, roll, "rzyx") d_point = rotation.dot(np.array([0.0, t, h])) - ref_line_point = get_point_xy_from_road_reference_line(road, s) + ref_line_point = get_point_xyz_from_road_reference_line(road, s) if ref_line_point is None: return None - return models.Point3D( + point_xyz = models.Point3D( x=ref_line_point.x + d_point[0], y=ref_line_point.y + d_point[1], - z=d_point[2], + z=ref_line_point.z + d_point[2], ) + + return point_xyz diff --git a/tests/test_utils.py b/tests/test_utils.py index 1249497..6a7b91a 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -131,13 +131,13 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0.0, ), ("simple_line_elevation.xodr", 0, 0, 0, 0, 0, 0), - ("simple_line_elevation.xodr", 5, 0, 0, 5, 0, 0), - ("simple_line_elevation.xodr", 5, 10, 0, 5, 10, 0), - ("simple_line_elevation.xodr", 5, -10, 0, 5, -10, 0), + ("simple_line_elevation.xodr", 5, 0, 0, 5, 0, 5), + ("simple_line_elevation.xodr", 5, 10, 0, 5, 10, 5), + ("simple_line_elevation.xodr", 5, -10, 0, 5, -10, 5), ("simple_line_heading_and_elevation.xodr", 0, 5, 0, -5, 0, 0), ("simple_line_heading_and_elevation.xodr", 0, -5, 0, 5, 0, 0), - ("simple_line_heading_and_elevation.xodr", 20, -5, 0, 5, 20, 0), - ("simple_line_heading_and_elevation.xodr", 20, 5, 0, -5, 20, 0), + ("simple_line_heading_and_elevation.xodr", 20, -5, 0, 5, 20, 20), + ("simple_line_heading_and_elevation.xodr", 20, 5, 0, -5, 20, 20), ( "Ex_Line-Spiral-Arc_elevation.xodr", 150, @@ -145,7 +145,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0, 74.12961916583495, 29.09285535716386, - 0, + 150, ), ( "Ex_Line-Spiral-Arc_elevation.xodr", @@ -154,7 +154,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0, 88.45632996588242, 15.137735950587523, - 0, + 150, ), ( "simple_line_elevation.xodr", @@ -235,7 +235,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0, -3.5355344833850797, 50, - 3.5355333282354717, + 53.5355333282354717, ), ( "simple_line_heading_and_elevation_and_superelevation.xodr", @@ -244,7 +244,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0, 3.53553448388698, 50, - -3.5355333282354717, + 50 - 3.5355333282354717, ), ( "Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr", @@ -253,7 +253,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0, 83.82560356938673, 19.64835535961951, - -3.5355333282354717, + 150 - 3.5355333282354717, ), ( "Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr", @@ -262,7 +262,16 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 0, 78.76034556233064, 24.58223594813187, - 3.5355333282354717, + 153.5355333282354717, + ), + ( + "Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr", + 150, + 5, + 10, + 83.82560191408653, + 19.648356971986246, + 160.6066022954953, ), ( "Ex_Line-Spiral-Arc_superelevation.xodr", @@ -282,6 +291,15 @@ def test_get_point_xyz_from_road_invalid_s() -> None: 24.58223594813187, 3.5355333282354717, ), + ( + "Ex_Line-Spiral-Arc_superelevation.xodr", + 150, + 5, + 10, + 83.82560191408653, + 19.648356971986246, + 10.60660229549529, + ), ], ) def test_get_point_xyz_from_road(file_name, s, t, h, x, y, z) -> None: