62 move waypoint along route

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

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

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

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:

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