From 6260e103c8ed717e9023fb580b9f2fe8db1c076d Mon Sep 17 00:00:00 2001 From: Gwilym-Rutherford Date: Tue, 5 Aug 2025 13:40:19 +0100 Subject: [PATCH 1/3] Add remaining distance to goal in autoware state --- srunner/autoagents/agent_state/autoware_state.py | 1 + srunner/autoagents/autoware_nodes/state_node.py | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/srunner/autoagents/agent_state/autoware_state.py b/srunner/autoagents/agent_state/autoware_state.py index b83d734..11dfe7b 100644 --- a/srunner/autoagents/agent_state/autoware_state.py +++ b/srunner/autoagents/agent_state/autoware_state.py @@ -51,3 +51,4 @@ def reset_state(self) -> None: self.goal_threshold: float = 0.5 self.initial_distance: float = -1 # distance reading when ADS reaches goal + self.remaining_distance: float = -1.0 diff --git a/srunner/autoagents/autoware_nodes/state_node.py b/srunner/autoagents/autoware_nodes/state_node.py index e912504..b0adb89 100644 --- a/srunner/autoagents/autoware_nodes/state_node.py +++ b/srunner/autoagents/autoware_nodes/state_node.py @@ -4,6 +4,7 @@ from autoware_adapi_v1_msgs.msg import LocalizationInitializationState from srunner.autoagents.agent_state import autoware_state from autoware_carla_interface_msgs.msg import EgoConfig, BridgeState +from autoware_internal_msgs.msg import MissionRemainingDistanceTime import logging @@ -34,7 +35,12 @@ def __init__(self, autoware_state: autoware_state.AutowareState) -> None: self.localize_state_cb, 10, ) - + self.remaining_time_distance_subscriber_ = self.create_subscription( + MissionRemainingDistanceTime, + "/planning/mission_remaining_distance_time", + self.time_distance_callback, + 10, + ) self.autoware_state_subscriber = self.create_subscription( BridgeState, self.bridge_state, self.bridge_state_cb, 10 ) @@ -81,3 +87,7 @@ def localize_state_cb( """ self.autoware_state.localize_state = localize_state_msg.state logger.info(f"Localization state: {self.autoware_state.localize_state}") + + def time_distance_callback(self, time_distance_msg): + self.autoware_state.remaining_distance = time_distance_msg.remaining_distance + self.remaining_time = time_distance_msg.remaining_time From 3be97bd65445627639f2c021dde9c7f6e418c529 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Tue, 5 Aug 2025 14:03:33 +0100 Subject: [PATCH 2/3] added logic to update route as you go along --- config.yaml | 2 +- .../autoagents/agent_state/autoware_state.py | 16 ++-- srunner/autoagents/autoware_agent.py | 27 ++++-- .../autoagents/autoware_nodes/route_node.py | 95 ++++++++++++++----- 4 files changed, 97 insertions(+), 43 deletions(-) diff --git a/config.yaml b/config.yaml index f13ae8d..229cc28 100644 --- a/config.yaml +++ b/config.yaml @@ -1,7 +1,7 @@ log: path: 'logs/' clear_old_logs: true - log_format: '[%(levelname)s] [%(asctime)s] [%(message)s] [%(filename)s]:[%(lineno)d]' + log_format: '[%(levelname)s] [%(asctime)s] %(message)s [%(filename)s]:[%(lineno)d]' carla: host: 127.0.0.1 port: 2000 diff --git a/srunner/autoagents/agent_state/autoware_state.py b/srunner/autoagents/agent_state/autoware_state.py index 11dfe7b..f283198 100644 --- a/srunner/autoagents/agent_state/autoware_state.py +++ b/srunner/autoagents/agent_state/autoware_state.py @@ -9,9 +9,9 @@ def __init__(self, name="", position=None): def within_goal(self) -> bool: return ( - self.distance_to_goal >= self.goal_threshold - or self.distance_to_goal == self.goal_threshold - ) + self.remaining_distance >= self.goal_threshold + or self.remaining_distance == self.goal_threshold + ) and (not (self.current_waypoint + 1) == self.total_waypoints) def completed_route(self) -> bool: return self.route_state == 6 or self.motion_state == 1 @@ -46,9 +46,9 @@ def reset_state(self) -> None: self.localize_state: int = 0 # goal state - self.achieved_goal: bool = False - self.distance_to_goal: float = -1 - self.goal_threshold: float = 0.5 - - self.initial_distance: float = -1 # distance reading when ADS reaches goal + self.goal_threshold: float = 5.0 + self.initial_distance: float = -1.0 # distance reading when ADS reaches goal self.remaining_distance: float = -1.0 + + self.current_waypoint = 0 + self.total_waypoints = 0 diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 8dd9e5a..bc547a9 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -82,12 +82,12 @@ def set_route(self) -> None: # publish self.agent_set_route = True - self.goal_pose_world = self._global_plan_world_coord[-1] - self.waypoints_world = self._global_plan_world_coord[:-1] + self.waypoints_world = self._global_plan_world_coord + + self.autoware_state.total_waypoints = len(self._global_plan_world_coord) # type: ignore # reinitialise localization logger.info("Localising Autoware agent...") - # self.autoware_node.request_localize() # None uses GNSS logger.info("Clearing route...") self.route_node.request_clear_route() @@ -126,17 +126,24 @@ def run_step(self) -> None: self.set_route() if self.autoware_state.is_ready_publish_route() and self.agent_set_route: - waypoints = [] goal_pose = self._convert_to_waypoint( - self.goal_pose_world + self.waypoints_world[self.autoware_state.current_waypoint] # type: ignore ).autoware_from_world_coords() - for waypoint in self.waypoints_world: - waypoints.append( - self._convert_to_waypoint(waypoint).autoware_from_world_coords() - ) - self.route_node.request_route(goal_pose, waypoints) + self.route_node.request_route(goal_pose, []) + self.autoware_state.current_waypoint += 1 # check if the current route is set if self.autoware_state.route_ready() and not self.autoware_state.sent_engage: self.autoware_node.publish_engage(True) + + # check we within some threshold distance + if self.autoware_state.within_goal(): + goal_pose = self._convert_to_waypoint( + self.waypoints_world[self.autoware_state.current_waypoint] # type: ignore + ).autoware_from_world_coords() + + self.route_node.change_route(goal_pose, []) + self.autoware_state.current_waypoint += 1 + else: + logger.info("Approaching last waypoint") diff --git a/srunner/autoagents/autoware_nodes/route_node.py b/srunner/autoagents/autoware_nodes/route_node.py index 8de0d96..01eee17 100644 --- a/srunner/autoagents/autoware_nodes/route_node.py +++ b/srunner/autoagents/autoware_nodes/route_node.py @@ -14,9 +14,9 @@ class RouteNode(Node): last_goal = None last_waypoints = [] - # 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" + change_route_points_service_name = "/api/routing/change_route_points" def __init__(self, autoware_state) -> None: # Initialize the Node base class with a unique name @@ -32,62 +32,96 @@ def __init__(self, autoware_state) -> None: ClearRoute, self.clear_route_service_name ) - # Good practice: Wait for the service server to be available before trying to call it - self.get_logger().info( - f"Waiting for '{self.set_route_points_service_name}' service..." + self.change_route_client = self.create_client( + SetRoutePoints, self.change_route_points_service_name ) + + # wait for the services to become available + while not self.set_route_client.wait_for_service(timeout_sec=1.0): self.get_logger().info( - f"Service '{self.set_route_points_service_name}' not available, waiting again..." + f"Waiting for '{self.set_route_points_service_name}' service..." ) self.get_logger().info( f"Service '{self.set_route_points_service_name}' available." ) - self.get_logger().info( - f"Waiting for '{self.clear_route_service_name}' service..." - ) while not self.clear_route_client.wait_for_service(timeout_sec=1.0): self.get_logger().info( - f"Service '{self.clear_route_service_name}' not available, waiting again..." + f"Waiting for '{self.clear_route_service_name}' service..." ) 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. + while not self.change_route_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info( + f"Waiting for '{self.change_route_points_service_name}' service..." + ) + self.get_logger().info( + f"Service '{self.change_route_points_service_name}' available." + ) + + def _assemble_route_msg( + self, goal: Pose, waypoints: list[Pose] + ) -> SetRoutePoints_Request: + """Build a SetRoutePoints_Request message for use with + - /api/routing/set_route_points + - /api/routing/change_route_points Args: - goal (Pose): The goal position. - waypoints (list[Pose]): A list of waypoints to pass through. - """ - self.get_logger().info("Sending route request...") + goal (Pose): Goal Pose + waypoints (list[Pose]): List of waypoints - # 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 + Returns: + SetRoutePoints_Request: Request Object + """ + request = SetRoutePoints_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 + return request + + def request_route(self, goal: Pose, waypoints: list[Pose]) -> None: + """Send a request to the /api/routing/set_route_points service. + + Args: + goal (Pose): The goal position. + waypoints (list[Pose]): A list of waypoints to pass through. + """ + self.get_logger().info("Sending route request...") + + request = self._assemble_route_msg(goal, 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 change_route(self, goal: Pose, waypoints: list[Pose]) -> None: + """Send a request to the /api/routing/change_route_points service. + + Args: + goal (Pose): The goal position. + waypoints (list[Pose]): A list of waypoints to pass through. + """ + self.get_logger().info("Sending route request...") + + request = self._assemble_route_msg(goal, waypoints) + + # Call the service asynchronously. This returns a Future object. + future = self.change_route_client.call_async(request) + + # Add a callback to process the response when it arrives. + future.add_done_callback(self.change_route_response_callback) + def set_route_response_callback(self, future): """Callback to handle the response from the SetRoutePoints service.""" try: @@ -115,6 +149,19 @@ def request_clear_route(self): future = self.clear_route_client.call_async(request) future.add_done_callback(self.clear_route_response_callback) + def change_route_response_callback(self, future): + """Callback to handle the response from the ChangeRoute service.""" + try: + response = future.result() + if response.status.success: + self.get_logger().info("Updated route successfully!") + else: + self.get_logger().warn( + f"Failed to update route, terminating scenario: {response.status.message}" + ) + except Exception as e: + self.get_logger().error(f"Service call failed: {e}") + def clear_route_response_callback(self, future): """Callback to handle the response from the ClearRoute service.""" try: From 334e5df8cf733cdb372c141db9c810888db23229 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Tue, 5 Aug 2025 16:19:06 +0100 Subject: [PATCH 3/3] added bug fix - needs test --- .../autoagents/agent_state/autoware_state.py | 9 ++++++-- srunner/autoagents/autoware_agent.py | 23 ++++++++++++++----- 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/srunner/autoagents/agent_state/autoware_state.py b/srunner/autoagents/agent_state/autoware_state.py index f283198..8a28600 100644 --- a/srunner/autoagents/agent_state/autoware_state.py +++ b/srunner/autoagents/agent_state/autoware_state.py @@ -9,10 +9,13 @@ def __init__(self, name="", position=None): def within_goal(self) -> bool: return ( - self.remaining_distance >= self.goal_threshold + self.remaining_distance <= self.goal_threshold or self.remaining_distance == self.goal_threshold ) and (not (self.current_waypoint + 1) == self.total_waypoints) + def in_motion(self) -> bool: + return self.route_ready and self.motion_state == 3 + def completed_route(self) -> bool: return self.route_state == 6 or self.motion_state == 1 @@ -40,13 +43,15 @@ def reset_state(self) -> None: self.sent_engage: bool = False self.bridge_ready: bool = False + self.planning: bool = False + # ADS state self.motion_state: int = 0 self.route_state: int = 0 self.localize_state: int = 0 # goal state - self.goal_threshold: float = 5.0 + self.goal_threshold: float = 25.0 self.initial_distance: float = -1.0 # distance reading when ADS reaches goal self.remaining_distance: float = -1.0 diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index bc547a9..70e764d 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -82,9 +82,9 @@ def set_route(self) -> None: # publish self.agent_set_route = True - self.waypoints_world = self._global_plan_world_coord + self.waypoints_world = self._global_plan_world_coord[1:] - self.autoware_state.total_waypoints = len(self._global_plan_world_coord) # type: ignore + self.autoware_state.total_waypoints = len(self.waypoints_world) # type: ignore # reinitialise localization logger.info("Localising Autoware agent...") @@ -137,13 +137,24 @@ def run_step(self) -> None: if self.autoware_state.route_ready() and not self.autoware_state.sent_engage: self.autoware_node.publish_engage(True) - # check we within some threshold distance - if self.autoware_state.within_goal(): + # if the route is set, we can plan again + if self.autoware_state.route_set(): + self.autoware_state.planning = False + + # check we are within some threshold distance and we are moving + # use the flag to only send once + if ( + self.autoware_state.within_goal() + and self.autoware_state.in_motion() + and not self.autoware_state.planning + ): + logger.info( + f"Updated goal_pose to waypoint number {self.autoware_state.current_waypoint}" + ) goal_pose = self._convert_to_waypoint( self.waypoints_world[self.autoware_state.current_waypoint] # type: ignore ).autoware_from_world_coords() self.route_node.change_route(goal_pose, []) self.autoware_state.current_waypoint += 1 - else: - logger.info("Approaching last waypoint") + self.autoware_state.planning = True