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 b83d734..8a28600 100644 --- a/srunner/autoagents/agent_state/autoware_state.py +++ b/srunner/autoagents/agent_state/autoware_state.py @@ -9,9 +9,12 @@ 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 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,14 +43,17 @@ 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.achieved_goal: bool = False - self.distance_to_goal: float = -1 - self.goal_threshold: float = 0.5 + self.goal_threshold: float = 25.0 + self.initial_distance: float = -1.0 # distance reading when ADS reaches goal + self.remaining_distance: float = -1.0 - self.initial_distance: float = -1 # distance reading when ADS reaches goal + self.current_waypoint = 0 + self.total_waypoints = 0 diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 8dd9e5a..70e764d 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[1:] + + self.autoware_state.total_waypoints = len(self.waypoints_world) # 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,35 @@ 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) + + # 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 + self.autoware_state.planning = True 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: 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