Skip to content

Commit

Permalink
Fix frame transform
Browse files Browse the repository at this point in the history
Signed-off-by: hoangtungdinh <[email protected]>
  • Loading branch information
hoangtungdinh committed Aug 27, 2024
1 parent 71aa2ac commit 0809dd1
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 23 deletions.
12 changes: 7 additions & 5 deletions qc_opendrive/base/utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -1540,8 +1540,7 @@ 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
pitch = get_pitch_from_road_reference_line(road, s)
roll = get_roll_from_road_reference_line(road, s)

if yaw is None or pitch is None or roll is None:
Expand All @@ -1550,12 +1549,15 @@ def get_point_xyz_from_road(
rotation = transforms3d.euler.euler2mat(yaw, pitch, 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
36 changes: 18 additions & 18 deletions tests/test_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -131,21 +131,21 @@ 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,
10,
0,
74.12961916583495,
29.09285535716386,
0,
150,
),
(
"Ex_Line-Spiral-Arc_elevation.xodr",
Expand All @@ -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",
Expand Down Expand Up @@ -233,36 +233,36 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
50,
5,
0,
-3.5355344833850797,
50,
3.5355333282354717,
-3.535534483397321,
47.500000408510964,
52.499999591506345,
),
(
"simple_line_heading_and_elevation_and_superelevation.xodr",
50,
-5,
0,
3.53553448388698,
50,
-3.5355333282354717,
52.499999591489036,
47.500000408493655,
),
(
"Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
150,
-5,
0,
83.82560356938673,
19.64835535961951,
-3.5355333282354717,
85.56999321017989,
21.43919391700692,
147.50000040849366,
),
(
"Ex_Line-Spiral-Arc_elevation_and_superelevation.xodr",
150,
5,
0,
78.76034556233064,
24.58223594813187,
3.5355333282354717,
77.01595592153748,
22.79139739074446,
152.49999959150634,
),
(
"Ex_Line-Spiral-Arc_superelevation.xodr",
Expand Down

0 comments on commit 0809dd1

Please sign in to comment.