diff --git a/opencda/core/common/cav_world.py b/opencda/core/common/cav_world.py
index 55316060d..e47bfd770 100644
--- a/opencda/core/common/cav_world.py
+++ b/opencda/core/common/cav_world.py
@@ -75,6 +75,18 @@ def update_vehicle_manager(self, vehicle_manager):
         self._vehicle_manager_dict.update(
             {vehicle_manager.vid: vehicle_manager})
 
+    def remove_vehicle_manager(self, vehicle_manager):
+        """
+        Remove a CAV manager from the world.
+
+        Parameters
+        ----------
+        vehicle_manager : opencda object
+            The vehicle manager class.
+        """
+        self.vehicle_id_set.discard(vehicle_manager.vehicle.id)
+        self._vehicle_manager_dict.pop(vehicle_manager.vid, None)
+
     def update_platooning(self, platooning_manger):
         """
         Add created platooning.
diff --git a/opencda/core/common/vehicle_manager.py b/opencda/core/common/vehicle_manager.py
index 9c67b5cea..f7bc597bd 100644
--- a/opencda/core/common/vehicle_manager.py
+++ b/opencda/core/common/vehicle_manager.py
@@ -94,6 +94,8 @@ def __init__(
         behavior_config = config_yaml['behavior']
         control_config = config_yaml['controller']
         v2x_config = config_yaml['v2x']
+        self.collision_detector_enabled = \
+            behavior_config.get('collision_detector_enabled', True)
 
         # v2x module
         self.v2x_manager = V2XManager(cav_world, v2x_config, self.vid)
@@ -136,6 +138,11 @@ def __init__(
         else:
             self.data_dumper = None
 
+        self.close_to_destination = False
+        self.to_start_location = False
+        self.route_start_location = None
+        self.route_end_location = None
+
         cav_world.update_vehicle_manager(self)
 
     def set_destination(
@@ -168,6 +175,31 @@ def set_destination(
         self.agent.set_destination(
             start_location, end_location, clean, end_reset)
 
+    def set_route_endpoints(self, start_location, end_location):
+        """
+        Save the two endpoints used when cycling a CAV route.
+        """
+        self.route_start_location = start_location
+        self.route_end_location = end_location
+
+    def set_close_to_destination(self, close_to_destination):
+        """
+        Set whether the vehicle has reached its destination.
+        """
+        self.close_to_destination = close_to_destination
+
+    def set_to_start_location(self, to_start_location):
+        """
+        Set whether the next destination is the start location.
+        """
+        self.to_start_location = to_start_location
+
+    def get_to_start_location(self):
+        """
+        Return whether the next destination is the start location.
+        """
+        return self.to_start_location
+
     def update_info(self):
         """
         Call perception and localization module to
@@ -211,7 +243,14 @@ def run_step(self, target_speed=None):
         """
         # visualize the bev map if needed
         self.map_manager.run_step()
-        target_speed, target_pos = self.agent.run_step(target_speed)
+        target_speed, target_pos = self.agent.run_step(
+            target_speed,
+            collision_detector_enabled=self.collision_detector_enabled)
+
+        if target_speed is None and target_pos is None:
+            self.set_close_to_destination(True)
+            return None
+
         control = self.controller.run_step(target_speed, target_pos)
 
         # dump data
diff --git a/opencda/core/plan/behavior_agent.py b/opencda/core/plan/behavior_agent.py
index b5375bb33..3276524d0 100644
--- a/opencda/core/plan/behavior_agent.py
+++ b/opencda/core/plan/behavior_agent.py
@@ -9,14 +9,13 @@
 
 import math
 import random
-import sys
 
 import numpy as np
 import carla
 
 from opencda.core.common.misc import get_speed, positive, cal_distance_angle
 from opencda.core.plan.collision_check import CollisionChecker
-from opencda.core.plan.local_planner_behavior import LocalPlanner
+from opencda.core.plan.local_planner_behavior import LocalPlanner, RoadOption
 from opencda.core.plan.global_route_planner import GlobalRoutePlanner
 from opencda.core.plan.global_route_planner_dao import GlobalRoutePlannerDAO
 from opencda.core.plan.planer_debug_helper import PlanDebugHelper
@@ -102,6 +101,18 @@ def __init__(self, vehicle, carla_map, config_yaml):
         self.emergency_param = config_yaml['emergency_param']
         self.break_distance = 0
         self.ttc = 1000
+        self.car_following_stop_distance = max(
+            0.0, float(config_yaml.get('car_following_stop_distance', 3.0)))
+        self.car_following_emergency_stop_distance = max(
+            0.0,
+            float(config_yaml.get('car_following_emergency_stop_distance', 3.0)))
+        self.car_following_creep_speed = float(
+            config_yaml.get('car_following_creep_speed', 6.0))
+        self.car_following_creep_time = max(
+            0.1, float(config_yaml.get('car_following_creep_time', 1.5)))
+        self.car_following_standstill_speed_thresh = max(
+            0.0,
+            float(config_yaml.get('car_following_standstill_speed_thresh', 0.3)))
         # collision checker
         self._collision_check = CollisionChecker(
             time_ahead=config_yaml['collision_time_ahead'])
@@ -239,6 +250,15 @@ def white_list_match(self, obstacles):
 
         return new_obstacle_list
 
+    def _get_driving_waypoint(self, location):
+        """
+        Project a location to a driving-lane waypoint.
+        """
+        return self._map.get_waypoint(
+            location,
+            project_to_road=True,
+            lane_type=carla.LaneType.Driving)
+
     def set_destination(
             self,
             start_location,
@@ -275,7 +295,7 @@ def set_destination(
         if clean_history:
             self.get_local_planner().get_history_buffer().clear()
 
-        self.start_waypoint = self._map.get_waypoint(start_location)
+        self.start_waypoint = self._get_driving_waypoint(start_location)
 
         # make sure the start waypoint is behind the vehicle
         if self._ego_pos:
@@ -289,7 +309,7 @@ def set_destination(
                 _, angle = cal_distance_angle(
                     self.start_waypoint.transform.location, cur_loc, cur_yaw)
 
-        end_waypoint = self._map.get_waypoint(end_location)
+        end_waypoint = self._get_driving_waypoint(end_location)
         if end_reset:
             self.end_waypoint = end_waypoint
 
@@ -501,7 +521,8 @@ def overtake_management(self, obstacle_vehicle):
             if not vehicle_state:
                 print("left overtake is operated")
                 self.overtake_counter = 100
-                next_wpt_list = left_wpt.next(self._ego_speed / 3.6 * 6)
+                lookahead_dist = max(self._ego_speed / 3.6 * 6.0, 0.5)
+                next_wpt_list = left_wpt.next(lookahead_dist)
                 if len(next_wpt_list) == 0:
                     return True
 
@@ -532,7 +553,8 @@ def overtake_management(self, obstacle_vehicle):
             if not vehicle_state:
                 print("right overtake is operated")
                 self.overtake_counter = 100
-                next_wpt_list = right_wpt.next(self._ego_speed / 3.6 * 6)
+                lookahead_dist = max(self._ego_speed / 3.6 * 6.0, 0.5)
+                next_wpt_list = right_wpt.next(lookahead_dist)
                 if len(next_wpt_list) == 0:
                     return True
 
@@ -603,10 +625,19 @@ def car_following_manager(self, vehicle, distance, target_speed=None):
         target_loc : carla.Location
             The target location.
         """
-        if not target_speed:
+        if target_speed is None:
             target_speed = self.max_speed - self.speed_lim_dist
 
         vehicle_speed = get_speed(vehicle)
+        if vehicle_speed <= self.car_following_standstill_speed_thresh:
+            gap = distance - self.car_following_stop_distance
+            if gap <= 0.0:
+                return 0.0
+            target_speed = min(
+                target_speed, gap / self.car_following_creep_time * 3.6)
+            if self.car_following_creep_speed > 0.0:
+                target_speed = min(target_speed, self.car_following_creep_speed)
+            return target_speed
 
         delta_v = max(1, (self._ego_speed - vehicle_speed) / 3.6)
         ttc = distance / delta_v if delta_v != 0 else distance / \
@@ -620,9 +651,7 @@ def car_following_manager(self, vehicle, distance, target_speed=None):
         # Actual safety distance area, try to follow the speed of the vehicle
         # in front.
         else:
-            target_speed = 0 if vehicle_speed == 0 else \
-                min(vehicle_speed + 1,
-                    target_speed)
+            target_speed = min(vehicle_speed + 1, target_speed)
         return target_speed
 
     def is_intersection(self, objects, waypoint_buffer):
@@ -661,9 +690,12 @@ def is_close_to_destination(self):
             It is True if the current ego vehicle's position is close to destination
 
         """
-        flag = abs(self._ego_pos.location.x - self.end_waypoint.transform.location.x) <= 10 and \
-               abs(self._ego_pos.location.y - self.end_waypoint.transform.location.y) <= 10
-        return flag
+        if self.end_waypoint is None:
+            return False
+
+        ego_loc = self._ego_pos.location
+        dest_loc = self.end_waypoint.transform.location
+        return math.hypot(ego_loc.x - dest_loc.x, ego_loc.y - dest_loc.y) <= 10.0
 
     def check_lane_change_permission(self, lane_change_allowed, collision_detector_enabled, rk):
         """
@@ -709,7 +741,7 @@ def check_lane_change_permission(self, lane_change_allowed, collision_detector_e
 
         return lane_change_allowed
 
-    def get_push_destination(self, ego_vehicle_wp, is_intersection):
+    def get_push_destination(self, ego_vehicle_wp, is_intersection, return_plan=False):
         """
         Get the destination for push operation.
 
@@ -727,18 +759,29 @@ def get_push_destination(self, ego_vehicle_wp, is_intersection):
             Temporal push destination.
 
         """
-        waypoint_buffer = self.get_local_planner().get_waypoint_buffer()
-        reset_index = len(waypoint_buffer) // 2
+        waypoint_buffer = list(self.get_local_planner().get_waypoint_buffer())
 
-        # when it comes to the intersection, we need to use the future
-        # waypoint to make sure the next waypoint is at the same lane
-        if is_intersection:
-            reset_target = waypoint_buffer[reset_index][0].next(
-                max(self._ego_speed / 3.6, 10.0))[0]
-        else:
-            reset_target = \
-                ego_vehicle_wp.next(max(self._ego_speed / 3.6 * 3,
-                                        10.0))[0]
+        if waypoint_buffer:
+            reset_index = max(0, min(len(waypoint_buffer) - 1,
+                                     len(waypoint_buffer) // 2))
+            reset_plan = waypoint_buffer[:reset_index + 1]
+
+            if is_intersection or not self._has_lane_change(reset_plan):
+                reset_target = reset_plan[-1][0]
+                if return_plan:
+                    return reset_target, reset_plan
+                return reset_target
+
+        reset_plan = self._build_lane_follow_plan(ego_vehicle_wp)
+        if reset_plan:
+            reset_target = reset_plan[-1][0]
+            if return_plan:
+                return reset_target, reset_plan
+            return reset_target
+
+        next_waypoints = ego_vehicle_wp.next(
+            max(self._ego_speed / 3.6 * 3.0, 10.0))
+        reset_target = next_waypoints[0] if next_waypoints else ego_vehicle_wp
         if self.debug:
             print(
                 'Vehicle id: %d :destination pushed forward because of '
@@ -746,8 +789,38 @@ def get_push_destination(self, ego_vehicle_wp, is_intersection):
                 (self.vehicle.id, reset_target.transform.location.x,
                  reset_target.transform.location.y,
                  reset_target.transform.location.z))
+        if return_plan:
+            return reset_target, None
         return reset_target
 
+    @staticmethod
+    def _has_lane_change(plan):
+        """
+        Return True if a waypoint plan already contains a lane-change option.
+        """
+        return any(option in (RoadOption.CHANGELANELEFT,
+                              RoadOption.CHANGELANERIGHT)
+                   for _, option in plan)
+
+    def _build_lane_follow_plan(self, start_waypoint):
+        """
+        Build a short lane-following plan without re-projecting through a map
+        location. This keeps push destinations on the same lane segment.
+        """
+        step_dist = max(self._ego_speed / 3.6, 2.0)
+        plan_dist = max(self._ego_speed / 3.6 * 3.0, 10.0)
+        steps = max(1, int(math.ceil(plan_dist / step_dist)))
+
+        plan = []
+        waypoint = start_waypoint
+        for _ in range(steps):
+            next_waypoints = waypoint.next(step_dist)
+            if not next_waypoints:
+                break
+            waypoint = next_waypoints[0]
+            plan.append((waypoint, RoadOption.LANEFOLLOW))
+        return plan
+
     def run_step(
             self,
             target_speed=None,
@@ -790,10 +863,10 @@ def run_step(
         # use traffic light to detect intersection
         is_intersection = self.is_intersection(self.objects, waipoint_buffer)
 
-        # 0. Simulation ends condition
+        # 0. Destination reached. Return an empty command target instead of
+        # exiting the whole process so each CAV can be handled independently.
         if self.is_close_to_destination():
-            print('Simulation is Over')
-            sys.exit(0)
+            return None, None
 
         # 1. Traffic light management
         if self.traffic_light_manager(ego_vehicle_wp) != 0:
@@ -823,10 +896,12 @@ def run_step(
 
         # Path generation based on the global route
         rx, ry, rk, ryaw = self._local_planner.generate_path()
+        if len(rx) == 0:
+            return 0, None
 
         # check whether lane change is allowed
-        self.lane_change_allowed = self.check_lane_change_permission(lane_change_allowed, collision_detector_enabled,
-                                                                     rk)
+        self.lane_change_allowed = self.check_lane_change_permission(
+            lane_change_allowed, collision_detector_enabled, rk)
 
         # 3. Collision check
         is_hazard = False
@@ -842,20 +917,38 @@ def run_step(
         # The case that the vehicle is doing lane change as planned
         # but found vehicle blocking on the other lane
         if not self.lane_change_allowed and \
-                self.get_local_planner().potential_curved_road \
+                self.get_local_planner().lane_id_change \
                 and not self.destination_push_flag and \
                 self.overtake_counter <= 0:
             self.overtake_allowed = False
-            # get push destination based on intersection flag and current waypoint (rule-based)
-            reset_target = self.get_push_destination(ego_vehicle_wp, is_intersection)
+            reset_target, reset_plan = self.get_push_destination(
+                ego_vehicle_wp, is_intersection, return_plan=True)
             # set the flag, so the push operation is not allowed for the next few frames.
             self.destination_push_flag = 90
-            self.set_destination(
-                ego_vehicle_loc,
-                reset_target.transform.location,
-                clean=True,
-                end_reset=False)
+            if reset_plan is not None:
+                local_planner = self.get_local_planner()
+                local_planner.get_waypoints_queue().clear()
+                local_planner.get_waypoint_buffer().clear()
+                local_planner.get_trajectory().clear()
+                local_planner.set_global_plan(reset_plan, clean=True)
+            else:
+                self.set_destination(
+                    ego_vehicle_loc,
+                    reset_target.transform.location,
+                    clean=True,
+                    end_reset=False)
             rx, ry, rk, ryaw = self._local_planner.generate_path()
+            if len(rx) == 0:
+                self.destination_push_flag = max(self.destination_push_flag, 300)
+                self.set_destination(
+                    ego_vehicle_loc,
+                    self.end_waypoint.transform.location,
+                    clean=True,
+                    end_reset=False,
+                    clean_history=True)
+                rx, ry, rk, ryaw = self._local_planner.generate_path()
+                if len(rx) == 0:
+                    return 0, None
 
         # 5. the case that vehicle is blocking in front and overtake not
         # allowed or it is doing overtaking the second condition is to
@@ -887,7 +980,8 @@ def run_step(
 
         # 7. Car following behavior
         if car_following_flag:
-            if distance < max(self.break_distance, 3):
+            if distance < max(self.break_distance,
+                              self.car_following_emergency_stop_distance):
                 return 0, None
 
             target_speed = self.car_following_manager(obstacle_vehicle, distance, target_speed)
@@ -898,5 +992,5 @@ def run_step(
         # 8. Normal behavior
         target_speed, target_loc = self._local_planner.run_step(
             rx, ry, rk, target_speed=self.max_speed - self.speed_lim_dist
-            if not target_speed else target_speed)
+            if target_speed is None else target_speed)
         return target_speed, target_loc
diff --git a/opencda/core/plan/local_planner_behavior.py b/opencda/core/plan/local_planner_behavior.py
index 04d803920..08e2f0597 100644
--- a/opencda/core/plan/local_planner_behavior.py
+++ b/opencda/core/plan/local_planner_behavior.py
@@ -237,9 +237,22 @@ def generate_path(self):
         x = []
         y = []
 
+        # BehaviorAgent calls generate_path() before run_step(); keep the
+        # local buffer populated before filtering and spline generation.
+        if len(self._waypoint_buffer) < self.waypoint_update_freq:
+            for _ in range(self._buffer_size - len(self._waypoint_buffer)):
+                if self.waypoints_queue:
+                    self._waypoint_buffer.append(
+                        self.waypoints_queue.popleft())
+                else:
+                    break
+
         # pop out the waypoints that may damage driving performance
         self.buffer_filter()
 
+        if len(self._waypoint_buffer) == 0:
+            return [], [], [], []
+
         # [m] distance of each interpolated points
         ds = 0.1
 
@@ -355,6 +368,8 @@ def generate_path(self):
 
         # we only need the interpolation points after current position
         s = np.arange(diff_s, sp.s[-1], ds)
+        if len(s) == 0:
+            return rx, ry, rk, ryaw
 
         self._long_plan_debug = []
         # we only need the interpolation points until next waypoint
diff --git a/opencda/scenario_testing/config_yaml/default.yaml b/opencda/scenario_testing/config_yaml/default.yaml
index 48a486bf8..a1384931a 100644
--- a/opencda/scenario_testing/config_yaml/default.yaml
+++ b/opencda/scenario_testing/config_yaml/default.yaml
@@ -118,6 +118,12 @@ vehicle_base:
     speed_decrease: 15 # used in car following mode to decrease speed for distance keeping
     safety_time: 4 # ttc safety thresholding for decreasing speed
     emergency_param: 0.4 # used to identify whether a emergency stop needed
+    collision_detector_enabled: true # whether to enable behavior-level collision checking
+    car_following_stop_distance: 3.0 # desired gap to a stopped lead vehicle, in meters
+    car_following_emergency_stop_distance: 3.0 # minimum gap to trigger immediate stop, in meters
+    car_following_creep_speed: 6.0 # max approach speed when the lead vehicle is stopped, km/h
+    car_following_creep_time: 1.5 # time constant for stopped-lead approach speed
+    car_following_standstill_speed_thresh: 0.3 # lead speed below this is treated as stopped, km/h
     ignore_traffic_light: true # whether to ignore traffic light
     overtake_allowed: true # whether overtake allowed, typically false for platoon leader
     collision_time_ahead: 1.5 # used for collision checking
@@ -182,5 +188,6 @@ platoon_base:
 
 # define tne scenario in each specific scenario
 scenario:
+  destination_reached_behavior: reset_destination # destroy or reset_destination
   single_cav_list: []
   platoon_list: []
diff --git a/opencda/scenario_testing/evaluations/evaluate_manager.py b/opencda/scenario_testing/evaluations/evaluate_manager.py
index 96265b577..fff03ae73 100644
--- a/opencda/scenario_testing/evaluations/evaluate_manager.py
+++ b/opencda/scenario_testing/evaluations/evaluate_manager.py
@@ -58,6 +58,10 @@ def evaluate(self):
         Evaluate performance of all modules by plotting and writing the
         statistics into the log file.
         """
+        if not self.cav_world.get_vehicle_managers():
+            print('No CAVs available for evaluation.')
+            return
+
         log_file = os.path.join(self.eval_save_path, 'log.txt')
 
         self.planning_eval(log_file)
diff --git a/opencda/scenario_testing/openscenario_carla.py b/opencda/scenario_testing/openscenario_carla.py
index 9d66a33c2..a16c8a353 100644
--- a/opencda/scenario_testing/openscenario_carla.py
+++ b/opencda/scenario_testing/openscenario_carla.py
@@ -74,6 +74,9 @@ def run_scenario(opt, scenario_params):
 
         while True:
             scenario_manager.tick()
+            if not single_cav_list:
+                break
+
             ego_cav = single_cav_list[0].vehicle
 
             # Bird view following
@@ -84,10 +87,8 @@ def run_scenario(opt, scenario_params):
             spectator.set_transform(view_transform)
 
             # Apply the control to the ego vehicle
-            for _, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
             time.sleep(0.01)
 
     finally:
diff --git a/opencda/scenario_testing/platoon_joining_2lanefree_carla.py b/opencda/scenario_testing/platoon_joining_2lanefree_carla.py
index 31d1df88c..795ea8139 100644
--- a/opencda/scenario_testing/platoon_joining_2lanefree_carla.py
+++ b/opencda/scenario_testing/platoon_joining_2lanefree_carla.py
@@ -75,14 +75,12 @@ def run_scenario(opt, scenario_params):
                 platoon.update_information()
                 platoon.run_step()
 
-            for i, single_cav in enumerate(single_cav_list):
+            for single_cav in list(single_cav_list):
                 # this function should be added in wrapper
                 if single_cav.v2x_manager.in_platoon():
-                    single_cav_list.pop(i)
+                    single_cav_list.remove(single_cav)
                 else:
-                    single_cav.update_info()
-                    control = single_cav.run_step()
-                    single_cav.vehicle.apply_control(control)
+                    scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/platoon_joining_2lanefree_cosim.py b/opencda/scenario_testing/platoon_joining_2lanefree_cosim.py
index cbbca5da1..954299036 100644
--- a/opencda/scenario_testing/platoon_joining_2lanefree_cosim.py
+++ b/opencda/scenario_testing/platoon_joining_2lanefree_cosim.py
@@ -75,13 +75,11 @@ def run_scenario(opt, scenario_params):
                 platoon.update_information()
                 platoon.run_step()
 
-            for i, single_cav in enumerate(single_cav_list):
+            for single_cav in list(single_cav_list):
                 if single_cav.v2x_manager.in_platoon():
-                    single_cav_list.pop(i)
-
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+                    single_cav_list.remove(single_cav)
+                else:
+                    scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/platoon_joining_town06_carla.py b/opencda/scenario_testing/platoon_joining_town06_carla.py
index ef3e93cf8..3c5f4da88 100644
--- a/opencda/scenario_testing/platoon_joining_town06_carla.py
+++ b/opencda/scenario_testing/platoon_joining_town06_carla.py
@@ -67,14 +67,12 @@ def run_scenario(opt, scenario_params):
                 platoon.update_information()
                 platoon.run_step()
 
-            for i, single_cav in enumerate(single_cav_list):
+            for single_cav in list(single_cav_list):
                 # this function should be added in wrapper
                 if single_cav.v2x_manager.in_platoon():
-                    single_cav_list.pop(i)
+                    single_cav_list.remove(single_cav)
                 else:
-                    single_cav.update_info()
-                    control = single_cav.run_step()
-                    single_cav.vehicle.apply_control(control)
+                    scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/single_2lanefree_carla.py b/opencda/scenario_testing/single_2lanefree_carla.py
index 5b485317b..c7f1d7106 100644
--- a/opencda/scenario_testing/single_2lanefree_carla.py
+++ b/opencda/scenario_testing/single_2lanefree_carla.py
@@ -59,6 +59,9 @@ def run_scenario(opt, scenario_params):
         # run steps
         while True:
             scenario_manager.tick()
+            if not single_cav_list:
+                break
+
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(
                 transform.location +
@@ -68,10 +71,8 @@ def run_scenario(opt, scenario_params):
                     pitch=-
                     90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/single_2lanefree_cosim.py b/opencda/scenario_testing/single_2lanefree_cosim.py
index 5d4221a59..b7287cfea 100644
--- a/opencda/scenario_testing/single_2lanefree_cosim.py
+++ b/opencda/scenario_testing/single_2lanefree_cosim.py
@@ -58,16 +58,16 @@ def run_scenario(opt, scenario_params):
         while True:
             # simulation tick
             scenario_manager.tick()
+            if not single_cav_list:
+                break
 
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(transform.location +
                                                     carla.Location(z=50),
                                                     carla.Rotation(pitch=-90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/single_intersection_town06_carla.py b/opencda/scenario_testing/single_intersection_town06_carla.py
index f64f09340..66f26bf8a 100644
--- a/opencda/scenario_testing/single_intersection_town06_carla.py
+++ b/opencda/scenario_testing/single_intersection_town06_carla.py
@@ -49,6 +49,9 @@ def run_scenario(opt, scenario_params):
         # run steps
         while True:
             scenario_manager.tick()
+            if not single_cav_list:
+                break
+
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(
                 transform.location +
@@ -58,10 +61,8 @@ def run_scenario(opt, scenario_params):
                     pitch=-
                     90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
@@ -75,4 +76,3 @@ def run_scenario(opt, scenario_params):
             v.destroy()
         for v in bg_veh_list:
             v.destroy()
-
diff --git a/opencda/scenario_testing/single_town05_cosim.py b/opencda/scenario_testing/single_town05_cosim.py
index d08d4c49d..de7859dda 100644
--- a/opencda/scenario_testing/single_town05_cosim.py
+++ b/opencda/scenario_testing/single_town05_cosim.py
@@ -50,16 +50,16 @@ def run_scenario(opt, scenario_params):
         while True:
             # simulation tick
             scenario_manager.tick()
+            if not single_cav_list:
+                break
 
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(transform.location +
                                                     carla.Location(z=50),
                                                     carla.Rotation(pitch=-90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/single_town06_carla.py b/opencda/scenario_testing/single_town06_carla.py
index e736be15c..07a9a87e1 100644
--- a/opencda/scenario_testing/single_town06_carla.py
+++ b/opencda/scenario_testing/single_town06_carla.py
@@ -46,6 +46,9 @@ def run_scenario(opt, scenario_params):
         # run steps
         while True:
             scenario_manager.tick()
+            if not single_cav_list:
+                break
+
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(
                 transform.location +
@@ -55,10 +58,8 @@ def run_scenario(opt, scenario_params):
                     pitch=-
                     90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
@@ -72,4 +73,3 @@ def run_scenario(opt, scenario_params):
             v.destroy()
         for v in bg_veh_list:
             v.destroy()
-
diff --git a/opencda/scenario_testing/single_town06_cosim.py b/opencda/scenario_testing/single_town06_cosim.py
index b88f939c8..ce68feeb9 100644
--- a/opencda/scenario_testing/single_town06_cosim.py
+++ b/opencda/scenario_testing/single_town06_cosim.py
@@ -50,16 +50,16 @@ def run_scenario(opt, scenario_params):
         while True:
             # simulation tick
             scenario_manager.tick()
+            if not single_cav_list:
+                break
 
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(transform.location +
                                                     carla.Location(z=50),
                                                     carla.Rotation(pitch=-90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
     finally:
         eval_manager.evaluate()
diff --git a/opencda/scenario_testing/utils/sim_api.py b/opencda/scenario_testing/utils/sim_api.py
index ead1973ae..7a4c67ecb 100644
--- a/opencda/scenario_testing/utils/sim_api.py
+++ b/opencda/scenario_testing/utils/sim_api.py
@@ -338,6 +338,9 @@ def create_vehicle_manager(self, application,
             destination = carla.Location(x=cav_config['destination'][0],
                                          y=cav_config['destination'][1],
                                          z=cav_config['destination'][2])
+            vehicle_manager.set_route_endpoints(
+                spawn_transform.location, destination)
+            vehicle_manager.set_to_start_location(True)
             vehicle_manager.update_info()
             vehicle_manager.set_destination(
                 vehicle_manager.vehicle.get_location(),
@@ -384,6 +387,9 @@ def create_vehicle_manager_from_scenario_runner(self, vehicle):
         destination = carla.Location(x=cav_config['destination'][0],
                                      y=cav_config['destination'][1],
                                      z=cav_config['destination'][2])
+        vehicle_manager.set_route_endpoints(
+            vehicle_manager.vehicle.get_location(), destination)
+        vehicle_manager.set_to_start_location(True)
         vehicle_manager.update_info()
         vehicle_manager.set_destination(
             vehicle_manager.vehicle.get_location(),
@@ -392,6 +398,85 @@ def create_vehicle_manager_from_scenario_runner(self, vehicle):
 
         return [vehicle_manager]
 
+    def get_destination_reached_behavior(self):
+        """
+        Return the configured behavior after a CAV reaches its destination.
+        """
+        scenario_config = self.scenario_params.get('scenario', {})
+        return str(
+            scenario_config.get(
+                'destination_reached_behavior',
+                'reset_destination')).lower()
+
+    def get_next_destination(self, vehicle_manager):
+        """
+        Return the next destination when cycling between route endpoints.
+        """
+        if (vehicle_manager.route_start_location is None or
+                vehicle_manager.route_end_location is None):
+            raise ValueError('CAV route endpoints are not configured.')
+
+        if vehicle_manager.get_to_start_location():
+            destination = vehicle_manager.route_start_location
+            vehicle_manager.set_to_start_location(False)
+        else:
+            destination = vehicle_manager.route_end_location
+            vehicle_manager.set_to_start_location(True)
+
+        return destination
+
+    def reset_cav_destination(self, vehicle_manager):
+        """
+        Send a CAV to the opposite endpoint after it reaches destination.
+        """
+        destination = self.get_next_destination(vehicle_manager)
+        vehicle_manager.set_close_to_destination(False)
+        vehicle_manager.set_destination(
+            vehicle_manager.vehicle.get_location(),
+            destination,
+            clean=True)
+
+    def destroy_cav(self, single_cav_list, vehicle_manager):
+        """
+        Destroy a CAV actor and remove its manager from active registries.
+        """
+        if vehicle_manager in single_cav_list:
+            single_cav_list.remove(vehicle_manager)
+        if self.cav_world:
+            self.cav_world.remove_vehicle_manager(vehicle_manager)
+        vehicle_manager.destroy()
+
+    def handle_cav_destination_reached(self, single_cav_list, vehicle_manager):
+        """
+        Apply configured behavior for a CAV that reached destination.
+        """
+        behavior = self.get_destination_reached_behavior()
+
+        if behavior == 'destroy':
+            self.destroy_cav(single_cav_list, vehicle_manager)
+        elif behavior in ('reset_destination', 'reset'):
+            self.reset_cav_destination(vehicle_manager)
+        else:
+            raise ValueError(
+                "scenario.destination_reached_behavior must be 'destroy' "
+                "or 'reset_destination'.")
+
+    def run_cav_step(self, single_cav_list, vehicle_manager):
+        """
+        Run one CAV step and handle destination arrival consistently.
+        """
+        vehicle_manager.update_info()
+        control = vehicle_manager.run_step()
+
+        if control is None:
+            if vehicle_manager.close_to_destination:
+                self.handle_cav_destination_reached(
+                    single_cav_list, vehicle_manager)
+            return None
+
+        vehicle_manager.vehicle.apply_control(control)
+        return control
+
     def create_platoon_manager(self, map_helper=None, data_dump=False):
         """
         Create a list of platoons.
diff --git a/opencda/scenario_testing/v2xp_datadump_town06_carla.py b/opencda/scenario_testing/v2xp_datadump_town06_carla.py
index 3f2fddcc6..28025d21b 100644
--- a/opencda/scenario_testing/v2xp_datadump_town06_carla.py
+++ b/opencda/scenario_testing/v2xp_datadump_town06_carla.py
@@ -62,6 +62,9 @@ def run_scenario(opt, scenario_params):
 
         while True:
             scenario_manager.tick()
+            if not single_cav_list:
+                break
+
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(
                 transform.location +
@@ -71,10 +74,8 @@ def run_scenario(opt, scenario_params):
                     pitch=-
                     90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
             for rsu in rsu_list:
                 rsu.update_info()
diff --git a/opencda/scenario_testing/v2xp_online_carla.py b/opencda/scenario_testing/v2xp_online_carla.py
index dd1b1e3a5..043875391 100644
--- a/opencda/scenario_testing/v2xp_online_carla.py
+++ b/opencda/scenario_testing/v2xp_online_carla.py
@@ -62,6 +62,9 @@ def run_scenario(opt, scenario_params):
 
         while True:
             scenario_manager.tick()
+            if not single_cav_list:
+                break
+
             transform = single_cav_list[0].vehicle.get_transform()
             spectator.set_transform(carla.Transform(
                 transform.location +
@@ -71,10 +74,8 @@ def run_scenario(opt, scenario_params):
                     pitch=-
                     90)))
 
-            for i, single_cav in enumerate(single_cav_list):
-                single_cav.update_info()
-                control = single_cav.run_step()
-                single_cav.vehicle.apply_control(control)
+            for single_cav in list(single_cav_list):
+                scenario_manager.run_cav_step(single_cav_list, single_cav)
 
             for rsu in rsu_list:
                 rsu.update_info()
@@ -93,4 +94,4 @@ def run_scenario(opt, scenario_params):
         for r in rsu_list:
             r.destroy()
         for v in bg_veh_list:
-            v.destroy()
\ No newline at end of file
+            v.destroy()