From 67980424cf98ddf6de6bffddc43cbb79f5c9b4a0 Mon Sep 17 00:00:00 2001
From: Gwilym-Rutherford <gmarutherford1@sheffield.ac.uk>
Date: Mon, 4 Aug 2025 15:49:58 +0100
Subject: [PATCH] Add orientation from CARLA api

---
 .../autoware_nodes/autoware_types/waypoint.py | 22 ++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py
index a16ffa9..a463d7b 100644
--- a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py
+++ b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py
@@ -33,12 +33,25 @@ def autoware_from_world_coords(self) -> Pose:
 
         orientation = self._get_orientation()
 
+        curr_location = carla.Location(self.x, self.y, self.z)
+        orientation = (
+            self.client.get_world()
+            .get_waypoint(
+                curr_location, project_to_road=True, lane_type=carla.LaneType.Driving
+            )
+            .transform.rotation
+        )
+
+        qx, qy, qz, qw = quaternion_from_euler(
+            orientation.roll, orientation.pitch, round(orientation.yaw, 1)
+        )
+
         pose.position = ros_point
         pose.orientation = Quaternion(
-            x=orientation["x"],
-            y=orientation["y"],
-            z=orientation["z"],
-            w=orientation["w"],
+            x=qx,
+            y=qy,
+            z=qz,
+            w=qw,
         )
 
         if self.marker_pub is not None:
@@ -50,7 +63,6 @@ def autoware_from_world_coords(self) -> Pose:
         return pose
 
     def _get_orientation(self) -> dict:
-        curr_location = carla.Location(self.x, self.y, self.z)
         point1 = (
             self.client.get_world()
             .get_map()