diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 0336fae..accb446 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/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 diff --git a/srunner/autoagents/autoware_nodes/autoware_node.py b/srunner/autoagents/autoware_nodes/autoware_node.py index e6ca672..76e95e1 100644 --- a/srunner/autoagents/autoware_nodes/autoware_node.py +++ b/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): diff --git a/srunner/autoagents/autoware_nodes/route_node.py b/srunner/autoagents/autoware_nodes/route_node.py index 603494a..fa01e11 100644 --- a/srunner/autoagents/autoware_nodes/route_node.py +++ b/srunner/autoagents/autoware_nodes/route_node.py @@ -62,49 +62,25 @@ 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" @@ -112,7 +88,12 @@ def _publish_goal(self, goal_point) -> None: 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" @@ -120,22 +101,6 @@ def _publish_checkpoint(self, checkpoint) -> None: 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...")