Add alternative publish route function

Author: Gwilym-Rutherford

srunner/autoagents/autoware_agent.py

@@ -10,6 +10,7 @@
 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
@@ -36,6 +37,7 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
 
         rclpy.init(args=None)
 
+
         self.autoware_state = autoware_state.AutowareState("ego_vehicle", None)
 
         self.route_node = route_node.RouteNode(self.autoware_state)
@@ -91,6 +93,8 @@ def set_route(self) -> None:
 
         logger.info("Clearing route...")
         self.route_node.request_clear_route()
+        
+        
 
     def _convert_to_waypoint(self, point):
         """Returns a waypoint
@@ -136,6 +140,7 @@ def run_step(self) -> None:
                     self._convert_to_waypoint(waypoint).autoware_from_world_coords()
                 )
             self.route_node.request_route(goal_pose, waypoints)
+            self.route_node.publish_route(goal_pose, waypoints)
 
         # check if the current route is set
         if self.autoware_state.route_ready() and not self.autoware_state.sent_engage:

srunner/autoagents/autoware_nodes/route_node.py

@@ -8,6 +8,7 @@
 # You might explicitly import the Request types for clarity, though not strictly necessary
 from autoware_adapi_v1_msgs.srv._set_route_points import SetRoutePoints_Request
 from autoware_adapi_v1_msgs.srv._clear_route import ClearRoute_Request
+from geometry_msgs.msg import PoseStamped
 
 
 class RouteNode(Node):
@@ -17,6 +18,9 @@ class RouteNode(Node):
     # These are service names, not topic names. Renamed for clarity.
     set_route_points_service_name = "/api/routing/set_route_points"
     clear_route_service_name = "/api/routing/clear_route"
+    
+    goal_topic = '/planning/mission_planning/goal'
+    checkpoint_topic = '/planning/mission_planning/checkpoint'
 
     def __init__(self, autoware_state) -> None:
         # Initialize the Node base class with a unique name
@@ -31,6 +35,13 @@ def __init__(self, autoware_state) -> None:
         self.clear_route_client = self.create_client(
             ClearRoute, self.clear_route_service_name
         )
+        
+        self.goal_publisher = self.create_publisher(
+            PoseStamped, self.goal_topic, 10
+        )
+        self.checkpoint_publisher = self.create_publisher(
+            PoseStamped, self.checkpoint_topic, 10
+        )
 
         # Good practice: Wait for the service server to be available before trying to call it
         self.get_logger().info(
@@ -87,6 +98,25 @@ def request_route(self, goal: Pose, waypoints: list[Pose]) -> None:
 
         # Add a callback to process the response when it arrives.
         future.add_done_callback(self.set_route_response_callback)
+        
+    def publish_route(self, goal: Pose, checkpoints: list[Pose]) -> None:
+        self._publish_goal(goal)
+        
+        for checkpoint in checkpoints:
+            self._publish_checkpoint(checkpoint)
+        
+    def _publish_goal(self, goal) -> None:
+        goal = PoseStamped()
+        goal.header.stamp = self.node.get_clock().now().to_msg()
+        goal.pose = goal
+        self.goal_publisher.publish(goal)
+        
+        
+    def _publish_checkpoint(self, checkpoint) -> None:
+        goal = PoseStamped()
+        goal.header.stamp = self.node.get_clock().now().to_msg()
+        goal.pose = checkpoint
+        self.checkpoint_publisher.publish(goal)
 
     def set_route_response_callback(self, future):
         """Callback to handle the response from the SetRoutePoints service."""