Get appropriate yaw value

srunner/autoagents/autoware_agent.py

@@ -102,7 +102,7 @@ def _convert_to_waypoint(self, point):
             point[0].location.x,
             point[0].location.y,
             point[0].location.z,
-            marker_pub=self.autoware_node.marker_publisher,
+            node=self.autoware_node,
         )
 
     def destroy(self) -> None:
@@ -130,6 +130,7 @@ def run_step(self) -> None:
             goal_pose = self._convert_to_waypoint(
                 self.goal_pose_world
             ).autoware_from_world_coords()
+
             for waypoint in self.waypoints_world:
                 waypoints.append(
                     self._convert_to_waypoint(waypoint).autoware_from_world_coords()

srunner/autoagents/autoware_nodes/autoware_types/waypoint.py

@@ -1,20 +1,24 @@
 import carla
-from math import atan2
 from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
 from visualization_msgs.msg import Marker
 from tf_transformations import quaternion_from_euler
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Point
 from geometry_msgs.msg import Quaternion
 
+import math
+import random
+
 
 class Waypoint(object):
-    def __init__(self, x, y, z, marker_pub=None):
+    def __init__(self, x, y, z, node=None):
         self.x = x
         self.y = y
         self.z = z
+        self.id = random.randint(0, 10000)
 
-        self.marker_pub = marker_pub
+        self.node = node
+        self.marker_pub = node.marker_publisher
 
         self.client = CarlaDataProvider.get_client()
 
@@ -31,19 +35,20 @@ def autoware_from_world_coords(self) -> Pose:
         ros_point.y = -self.y
         ros_point.z = self.z
 
-        orientation = self._get_orientation()
-
         curr_location = carla.Location(self.x, self.y, self.z)
         orientation = (
             self.client.get_world()
+            .get_map()
             .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)
+            math.radians(orientation.roll),
+            -math.radians(orientation.pitch),
+            -math.radians(round(orientation.yaw, 1)),
         )
 
         pose.position = ros_point
@@ -54,53 +59,27 @@ def autoware_from_world_coords(self) -> Pose:
             w=qw,
         )
 
+        self.pose = pose
+
         if self.marker_pub is not None:
-            print(self)
             self.marker_pub.publish(self._publish_marker(pose))
 
-        self.pose = pose
-
         return pose
 
-    def _get_orientation(self) -> dict:
-        point1 = (
-            self.client.get_world()
-            .get_map()
-            .get_waypoint(
-                curr_location, project_to_road=True, lane_type=carla.LaneType.Driving
-            )
-        )
-
-        # get next waypoint that is 0.5 meters away
-        point2 = point1.next(0.5)[0]
-
-        dx = point2.transform.location.x - point1.transform.location.x
-        dy = point2.transform.location.y - point1.transform.location.y
-        yaw = atan2(dy, dx)
-
-        # rounding is need by autoware to function properly
-        qx, qy, qz, qw = quaternion_from_euler(0, 0, round(yaw, 1))
-
-        return {
-            "x": qx,
-            "y": qy,
-            "z": qz,
-            "w": qw,
-        }
-
     def _publish_marker(self, pose):
         marker = Marker()
-        marker.header.frame_id = "/map"
-        marker.header.stamp = self.get_clock().now().to_msg()
+        marker.header.frame_id = "map"
+        marker.header.stamp = self.node.get_clock().now().to_msg()
 
-        marker.type = marker.ARROW
+        marker.type = marker.SPHERE
         marker.action = marker.ADD
+        marker.id = self.id
 
         marker.pose = pose
 
-        marker.scale.x = 0.5
-        marker.scale.y = 0.05
-        marker.scale.z = 1
+        marker.scale.x = 1.0
+        marker.scale.y = 0.4
+        marker.scale.z = 1.0
 
         marker.color.r = 1.0
         marker.color.g = 0.0