revert to old waypoint visualizer

srunner/autoagents/autoware_agent.py

@@ -10,7 +10,6 @@
 from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig
 
 from autoware_carla_interface_msgs.msg import EgoConfig, SensorConfig
-from geometry_msgs.msg import PoseStamped
 
 import threading
 import rclpy
@@ -147,5 +146,7 @@ def run_step(self) -> None:
             self.sent_route = True
 
         # check if the current route is set
+        logger.info("About to publish engage")
         if self.autoware_state.route_ready() and not self.autoware_state.sent_engage:
+            logger.info("publishing engage")
             self.autoware_node.publish_engage(True)

srunner/autoagents/autoware_nodes/autoware_node.py

@@ -7,7 +7,7 @@
     InitializeLocalization_Request,
 )  # Explicitly import Request
 from geometry_msgs.msg import PoseWithCovarianceStamped
-from nav_msgs.msg import Path
+from visualization_msgs.msg import Marker
 
 # Assuming this import path is correct for your project
 from srunner.autoagents.agent_state import autoware_state
@@ -33,7 +33,9 @@ def __init__(
         )
 
         # marker publisher
-        self.path_publisher = self.create_publisher(Path, "path", 10)
+        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...")

srunner/autoagents/autoware_nodes/autoware_types/waypoint.py

@@ -18,7 +18,7 @@ def __init__(self, x, y, z, node=None):
         self.id = random.randint(0, 10000)
 
         self.node = node
-        self.path_publisher = node.path_publisher
+        self.marker_pub = node.marker_publisher
 
         self.client = CarlaDataProvider.get_client()
 
@@ -61,16 +61,32 @@ def autoware_from_world_coords(self) -> Pose:
 
         self.pose = pose
 
+        if self.marker_pub is not None:
+            self.marker_pub.publish(self._publish_marker(pose))
+
         return pose
 
-    def publish_route(self, poses: list[Pose]) -> None:
-        path = Marker()
-        path.header.frame_id = "map"
-        path.header.stamp = self.node.get_clock().now().to_msg()
+    def _publish_marker(self, pose):
+        marker = Marker()
+        marker.header.frame_id = "map"
+        marker.header.stamp = self.node.get_clock().now().to_msg()
+
+        marker.type = marker.SPHERE
+        marker.action = marker.ADD
+        marker.id = self.id
+
+        marker.pose = pose
+
+        marker.scale.x = 1.0
+        marker.scale.y = 0.4
+        marker.scale.z = 1.0
 
-        path.poses = poses
+        marker.color.r = 1.0
+        marker.color.g = 0.0
+        marker.color.b = 0.0
+        marker.color.a = 1.0
 
-        self.path_publisher.publish(path)
+        return marker
 
     def __str__(self) -> str:
         if self.pose: