diff --git a/docker/autoware_msgs.tar b/docker/autoware_msgs.tar index 41de37f..cdbe35a 100644 Binary files a/docker/autoware_msgs.tar and b/docker/autoware_msgs.tar differ diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 6182327..7fecbd7 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -60,6 +60,10 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: self.publish_sensor_state() + self.setup_tick_service() + + self.setup_route() + def publish_sensor_state(self) -> None: # publish sensor information to the bridge # wait for it to return the correct message @@ -78,7 +82,7 @@ def publish_sensor_state(self) -> None: # big performance diminishment here while not self.autoware_state.bridge_ready: logger.info("Sending Sensor state to Agent...") - time.sleep(5) # DO NOT CHANGE THIS IS A MAGIC NUMBER + time.sleep(5) # DO NOT CHANGE THIS IS A MAGIC NUMBER self.state_node.ego_config_publisher.publish(ego_config_msg) def set_route(self) -> None: @@ -123,7 +127,7 @@ def run_step(self) -> None: self.counter += 1 if self.counter % 20 == 0: logger.info("Ticked 1 second") - + if not self.agent_set_route: self.set_route() @@ -152,4 +156,4 @@ def run_step(self) -> None: # check if the current route is set if self.autoware_state.route_set() and not self.autoware_state.sent_engage: - self.autoware_node.publish_engage(True) + self.autoware_node.publish_engage(True) diff --git a/srunner/autoagents/autoware_nodes/autoware_node.py b/srunner/autoagents/autoware_nodes/autoware_node.py index e92c3a4..acdc379 100644 --- a/srunner/autoagents/autoware_nodes/autoware_node.py +++ b/srunner/autoagents/autoware_nodes/autoware_node.py @@ -15,25 +15,18 @@ class AutowareNode(Node): - engage_topic = "/autoware/engage" # Renamed for clarity as it's a topic - localize_service = ( - "/api/localization/initialize" # Renamed for clarity as it's a service - ) + engage_topic = "/autoware/engage" + localize_service = "/api/localization/initialize" - def __init__( - self, autoware_state_instance: autoware_state.AutowareState - ): # Renamed arg for clarity - super().__init__("autoware_node") # Initialize the Node with a unique name + def __init__(self, autoware_state_instance: autoware_state.AutowareState): + super().__init__("autoware_node") - # This is correct: Engage is a message type for a topic self.engage_publisher = self.create_publisher(Engage, self.engage_topic, 10) - # FIX: Create a service client, not a publisher, for InitializeLocalization self.localize_client = self.create_client( InitializeLocalization, self.localize_service ) - # marker publisher self.marker_publisher = self.create_publisher( Marker, "visulaization_marker", 10 ) @@ -77,24 +70,15 @@ def request_localize( """ self.get_logger().info("Sending localization initialization request...") - # Create a request object for the InitializeLocalization service - request = ( - InitializeLocalization_Request() - ) # InitializeLocalization() would also work - + request = InitializeLocalization_Request() if global_pose: # Populate the request with the provided pose # Assuming the service definition has a field named 'pose' of type PoseWithCovarianceStamped request.pose = global_pose else: - # If no pose is provided, send an empty request (as per Autoware behavior) - # The 'pose' field of the request message would default to its empty state. self.get_logger().info( "No initial global_pose provided, sending empty localization request." ) - # If the service requires 'pose' to always be set, even to a default, - # you'd need to explicitly set a default PoseWithCovarianceStamped here. - # Example: request.pose = PoseWithCovarianceStamped() # Creates an empty message # Call the service asynchronously future = self.localize_client.call_async(request) diff --git a/srunner/autoagents/autoware_nodes/tick_node.py b/srunner/autoagents/autoware_nodes/tick_node.py new file mode 100644 index 0000000..d7bf438 --- /dev/null +++ b/srunner/autoagents/autoware_nodes/tick_node.py @@ -0,0 +1,31 @@ +# import message types + +from rclpy.node import Node + + +class TickNode(Node): + """ROS2 Client Node solely responsible for ticking the Autoware-Carla-Bridge. + The service is called syncronously, blocking execution until Autoware executes the action and replies. + + The node also has an optional boolean to enable tracking execution time. + """ + + tick_service = "/autoware/tick" + + def __init__(self, exec_time: bool = False, debug: bool = False) -> None: + super().__init__("") + self._exec_time = exec_time + self.debug = debug + + self.tick_client = self.create_client( + # message type - implement, + self.tick_service + ) + + def autoware_tick(self) -> None: + # get current time + # assemble tick message + # over client + # block until response received + + return