diff --git a/src/Active-Lane-Keeping-Assistant/agent.py b/src/Active-Lane-Keeping-Assistant/agent.py index 18a0706b7a..686d2819eb 100644 --- a/src/Active-Lane-Keeping-Assistant/agent.py +++ b/src/Active-Lane-Keeping-Assistant/agent.py @@ -1,17 +1,23 @@ from __future__ import annotations +import os import matplotlib.pyplot as plt import numpy as np -import os class Agent(): """Agent that outputs the desired behaviour given """ SIMPLE_STEER_LIMIT = 0.75 SIMPLE_ERROR_TOLERANCE = 0.1 - def __init__(self, tau_p:float = 0, tau_d:float = 0, tau_i:float = 0, - surface_lower_threshold:float = 20e6, throttle:float = 0.3, - suface_upper_threshold=30e6, controller:str = 'simple') -> None: + + def __init__(self, + tau_p: float = 0, + tau_d: float = 0, + tau_i: float = 0, + surface_lower_threshold: float = 20e6, + throttle: float = 0.3, + surface_upper_threshold: float = 30e6, # Fixed typo: suface -> surface + controller: str = 'simple') -> None: """Constructor Args: @@ -43,7 +49,7 @@ def __init__(self, tau_p:float = 0, tau_d:float = 0, tau_i:float = 0, self.tau_d = tau_d self.tau_i = tau_i self.surface_lower_threshold = surface_lower_threshold - self.surface_upper_threshold = suface_upper_threshold + self.surface_upper_threshold = surface_upper_threshold # Fixed assignment self.prev_error = None self.throttle = throttle self.func = None @@ -115,27 +121,22 @@ def save_error_fig(self, path:str, id:str) -> None: def get_actions(self, detection_surface_area:float, error:float) -> tuple[float, float]: - """Retrieve action based on Observation - - Args: - error (float): Difference to the center of the detected lane. - detection_surface_area (float): Detected surface area. - Returns: - tuple[float, float]: - [0]: Steering angle to use. - [1]: Throttle to apply. - """ if (self.check_surface_area(detection_surface_area)): steer = 0 - if len(self.errors) > 0: + if self.errors: self.errors.append(self.errors[-1]) else: self.errors.append(0) else: self.errors.append(error) steer = self.func(error=error) - return steer, self.throttle + # [Planning] Adaptive Cruise Control (ACC) logic + # Decrease throttle when steering angle is high (cornering) + # Increase throttle when steering angle is low (straight line) + dynamic_throttle = max(0.2, self.throttle - abs(steer) * 0.4) + + return steer, dynamic_throttle @staticmethod def _simple_controller(error: float) -> float: @@ -179,9 +180,10 @@ def _pd_controller(self, error:float) -> float: self.prev_error = self.errors[-2] else: self.prev_error = self.errors[-1] - + deviation = error - self.prev_error - steer = - self.tau_d * deviation/0.1 + self._p_controller(error) + dt = 0.1 # Delta time + steer = - self.tau_d * deviation / dt + self._p_controller(error) return steer def _pid_controller(self, error:float) -> float: diff --git a/src/Active-Lane-Keeping-Assistant/lane.py b/src/Active-Lane-Keeping-Assistant/lane.py index ddfc45eab6..88d41a1877 100644 --- a/src/Active-Lane-Keeping-Assistant/lane.py +++ b/src/Active-Lane-Keeping-Assistant/lane.py @@ -16,6 +16,9 @@ class Lane: ROI_TOP = 260 ROI_LEFT = 200 ROI_RIGHT = 440 + # === 新增 === + MARGIN_RATIO = 1.0 / 12.0 + # =========== def __init__(self, height:int, width:int, save:bool=False, save_folder:str=join('img', 'examples')) -> None: """Constructor @@ -32,7 +35,7 @@ def __init__(self, height:int, width:int, save:bool=False, self.save_folder = save_folder self.img_height = height self.img_width = width - self.margin = int((1/12) * self.img_width) + self.margin = int(self.MARGIN_RATIO * self.img_width) self.DEGREE = 2 # Variables to store for lines @@ -63,7 +66,14 @@ def get_lines(self, img:np.ndarray) -> np.ndarray: # Create threshold matrix to differentiate black and white based on the # lightness (of HSL). _, binary = cv2.threshold(hls[:, :, 1], self.HLS_L_THRESHOLD_MIN, self.HLS_L_THRESHOLD_MAX, cv2.THRESH_BINARY) - binary_blured = cv2.GaussianBlur(binary, (3, 3), 0) + + # [Perception] Apply Morphological Closing to fill gaps in lane lines + # and remove small noise dots. + kernel = np.ones((5, 5), np.uint8) + binary_closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel) + + # Blur the processed image + binary_blured = cv2.GaussianBlur(binary_closed, (3, 3), 0) if self.save: cv2.imwrite(join(self.save_folder, 'lines.jpg'), binary_blured) diff --git a/src/Active-Lane-Keeping-Assistant/world.py b/src/Active-Lane-Keeping-Assistant/world.py index 37b7d25488..15846afd94 100644 --- a/src/Active-Lane-Keeping-Assistant/world.py +++ b/src/Active-Lane-Keeping-Assistant/world.py @@ -109,7 +109,8 @@ def reset(self) -> tuple[float, float, np.ndarray, bool]: self.collision_sensor = self.world.spawn_actor(blueprint, carla.Transform(), attach_to=self.vehicle) self.collision_detected = False - self.collision_sensor.listen(lambda e: {self._set_collision()}) + # Remove redundant set brackets + self.collision_sensor.listen(lambda e: self._set_collision()) self.initialized = True return 0.0, 0.0, np.zeros((self.image_height, self.image_width, 3)), False @@ -203,8 +204,8 @@ def step(self, show: bool = True, throttle: float = 0, transformed_image, error, detection_surface_area = self.lane \ .pipe(img=image) - #if show: - #World.show_image(image=transformed_image) + if show: + World.show_image(image=transformed_image) return error, detection_surface_area, transformed_image, \ self.collision_detected \ No newline at end of file