Merge pull request #59 from Intelligent-Testing-Lab/get-appropriate-yaw-value

added waypoints

Author: david-gasinski

srunner/autoagents/autoware_agent.py

@@ -102,6 +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,
         )
 
     def destroy(self) -> None:

srunner/autoagents/autoware_nodes/autoware_node.py

@@ -7,6 +7,7 @@
     InitializeLocalization_Request,
 )  # Explicitly import Request
 from geometry_msgs.msg import PoseWithCovarianceStamped
+from visualization_msgs.msg import Marker
 
 # Assuming this import path is correct for your project
 from srunner.autoagents.agent_state import autoware_state
@@ -31,6 +32,11 @@ def __init__(
             InitializeLocalization, self.localize_service
         )
 
+        # marker publisher
+        self.marker_publisher = self.create_publisher(
+            Marker, "visulaization_marker", 10
+        )
+
         # Good practice: Wait for the service to be available
         self.get_logger().info(f"Waiting for '{self.localize_service}' service...")
         while not self.localize_client.wait_for_service(timeout_sec=1.0):

srunner/autoagents/autoware_nodes/autoware_types/waypoint.py

@@ -9,16 +9,14 @@
 
 
 class Waypoint(object):
-    def __init__(self, x, y, z):
+    def __init__(self, x, y, z, marker_pub=None):
         self.x = x
         self.y = y
         self.z = z
 
-        self.client = CarlaDataProvider.get_client()
+        self.marker_pub = marker_pub
 
-        self.marker_publisher = self.create_publisher(
-            Marker, "visulaization_marker", 10
-        )
+        self.client = CarlaDataProvider.get_client()
 
     def autoware_from_world_coords(self) -> Pose:
         """convert from carla world coordinates to autoware waypoints
@@ -42,8 +40,12 @@ def autoware_from_world_coords(self) -> Pose:
             z=orientation["z"],
             w=orientation["w"],
         )
-    
-        self._publish_marker()
+
+        if self.marker_pub is not None:
+            print(self)
+            self.marker_pub.publish(self._publish_marker(pose))
+
+        self.pose = pose
 
         return pose
 
@@ -93,4 +95,9 @@ def _publish_marker(self, pose):
         marker.color.b = 0.0
         marker.color.a = 1.0
 
-        self.marker_publisher.publish(marker)
+        return marker
+
+    def __str__(self) -> str:
+        if self.pose:
+            return f"{self.pose}"
+        return f"x: {self.x}, y: {-self.y}, z: {self.z}"