Add empty message to restart AW

srunner/autoagents/autoware_agent.py

@@ -11,6 +11,8 @@
 
 from autoware_carla_interface_msgs.msg import EgoConfig, SensorConfig
 
+from std_msgs.msg import Empty
+
 import threading
 import rclpy
 import time
@@ -113,11 +115,16 @@ def destroy(self) -> None:
             self.autoware_node.destroy()
             self.state_node.destroy()
             self.route_node.destroy()
+            self.send_reset_bridge_signal()
             rclpy.shutdown()
             self._executor_thread.join()
         except RuntimeError:
             logger.info("Cleaned up threads...")
 
+    def send_reset_bridge_signal(self) -> None:
+        message = Empty()
+        self.autoware_node.reset_bridge_publisher.publish(message)
+
     def run_step(self) -> None:
         """Tick method containing all logic based on autoware state"""
         self.counter += 1

srunner/autoagents/autoware_nodes/autoware_node.py

@@ -9,6 +9,8 @@
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from visualization_msgs.msg import Marker
 
+from std_msgs.msg import Empty
+
 # Assuming this import path is correct for your project
 from srunner.autoagents.agent_state import autoware_state
 
@@ -18,6 +20,7 @@ class AutowareNode(Node):
     localize_service = (
         "/api/localization/initialize"  # Renamed for clarity as it's a service
     )
+    reset_topic = "/bridge/reset"
 
     def __init__(
         self, autoware_state_instance: autoware_state.AutowareState
@@ -37,6 +40,8 @@ def __init__(
             Marker, "visulaization_marker", 10
         )
 
+        self.reset_bridge_publisher = self.create_publisher(Empty, self.reset_topic, 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/route_node.py

@@ -62,80 +62,45 @@ def __init__(self, autoware_state) -> None:
             )
         self.get_logger().info(f"Service '{self.clear_route_service_name}' available.")
 
-    def request_route(self, goal: Pose, waypoints: list[Pose]) -> None:
-        """Send a request to the /api/routing/set_route_points service.
+    def publish_route(self, goal: Pose, checkpoints: list[Pose]) -> None:
+        """Responsible for publishing the end goal point and the obligatory checkpoints to visist
 
         Args:
-            goal (Pose): The goal position.
-            waypoints (list[Pose]): A list of waypoints to pass through.
+            goal (Pose): goal position
+            checkpoints (list[Pose]): checkpoints to visit before end goal
         """
-        self.get_logger().info("Sending route request...")
-
-        # Create a request object for the SetRoutePoints service
-        # The request message structure is defined in the .srv file.
-        # It's typically accessed as ServiceType.Request or by instantiating ServiceType()
-        request = (
-            SetRoutePoints_Request()
-        )  # Or just SetRoutePoints() as it defaults to Request
-
-        header_msg = Header()
-        time_stamp = self.get_clock().now().to_msg()
-
-        header_msg.frame_id = "map"
-        # FIX: Assign the timestamp to header_msg.stamp, not frame_id.
-        header_msg.stamp = time_stamp
-
-        request.header = header_msg
-        # FIX: The field name in SetRoutePoints.srv is likely 'goal', not 'pose'.
-        request.goal = goal
-        # FIX: Corrected typo from 'wayponts' to 'waypoints'.
-        request.waypoints = waypoints
-
-        # Call the service asynchronously. This returns a Future object.
-        future = self.set_route_client.call_async(request)
-
-        # 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:
 
         for checkpoint in checkpoints:
             self._publish_checkpoint(checkpoint)
-        
+
         self._publish_goal(goal)
 
-    def _publish_goal(self, goal_point) -> None:
+    def _publish_goal(self, goal_point: Pose) -> None:
+        """publish the goal position
+
+        Args:
+            goal_point (Pose): end goal position
+        """
         goal = PoseStamped()
         goal.header.stamp = self.get_clock().now().to_msg()
         goal.header.frame_id = "map"
 
         goal.pose = goal_point
         self.goal_publisher.publish(goal)
 
-    def _publish_checkpoint(self, checkpoint) -> None:
+    def _publish_checkpoint(self, checkpoint: Pose) -> None:
+        """publish a single checkpoint position
+
+        Args:
+            checkpoint (Pose): checkpoint position
+        """
         checkpoint_msg = PoseStamped()
         checkpoint_msg.header.stamp = self.get_clock().now().to_msg()
         checkpoint_msg.header.frame_id = "map"
 
         checkpoint_msg.pose = checkpoint
         self.checkpoint_publisher.publish(checkpoint_msg)
 
-    def set_route_response_callback(self, future):
-        """Callback to handle the response from the SetRoutePoints service."""
-        try:
-            # Get the result from the future object
-            response = future.result()
-            # Assuming Autoware services return a status field with success/message
-            if response.status.success:
-                self.autoware_state.sent_route = True
-                self.get_logger().info("Route set successfully!")
-            else:
-                self.get_logger().warn(
-                    f"Failed to set route: {response.status.message}"
-                )
-        except Exception as e:
-            self.get_logger().error(f"Service call failed: {e}")
-
     def request_clear_route(self):
         """Send a request to clear the route by calling /api/routing/clear_route service."""
         self.get_logger().info("Sending clear route request...")