diff --git a/src/Active-Lane-Keeping-Assistant/agent.py b/src/Active-Lane-Keeping-Assistant/agent.py index 6c51f98c18..18a0706b7a 100644 --- a/src/Active-Lane-Keeping-Assistant/agent.py +++ b/src/Active-Lane-Keeping-Assistant/agent.py @@ -138,24 +138,21 @@ def get_actions(self, detection_surface_area:float, return steer, self.throttle @staticmethod - def _simple_controller(error:float) -> float: - """Hard Coded Controller - - Args: - error (float): Difference to the center of the detected lane. - - Returns: - float: Steering angle to use. + def _simple_controller(error: float) -> float: + """Hard Coded Controller - Optimized to use Class Constants """ - limit = 0.75 - tolerance = 0.1 + # 使用类中定义的常量,方便统一管理 + limit = Agent.SIMPLE_STEER_LIMIT + tolerance = Agent.SIMPLE_ERROR_TOLERANCE + if (abs(error) < tolerance): - steer = 0 - elif error > 0: - steer = -limit - else: - steer = limit - return steer + return 0.0 + + # 简化逻辑分支 + if error > 0: + return -limit + + return limit def _p_controller(self, error:float) -> float: """Proportional Controller diff --git a/src/Active-Lane-Keeping-Assistant/lane.py b/src/Active-Lane-Keeping-Assistant/lane.py index 9a7cd67f09..ddfc45eab6 100644 --- a/src/Active-Lane-Keeping-Assistant/lane.py +++ b/src/Active-Lane-Keeping-Assistant/lane.py @@ -12,6 +12,10 @@ class Lane: HLS_L_THRESHOLD_MIN = 150 HLS_L_THRESHOLD_MAX = 255 XM_PER_PIX = 3.7 / 781 + ROI_BOTTOM = 350 + ROI_TOP = 260 + ROI_LEFT = 200 + ROI_RIGHT = 440 def __init__(self, height:int, width:int, save:bool=False, save_folder:str=join('img', 'examples')) -> None: """Constructor @@ -66,8 +70,11 @@ def get_lines(self, img:np.ndarray) -> np.ndarray: return binary_blured - def extract_roi(self, img:np.ndarray, bottom:int=350, top:int=260, - left:int=200, right:int=440) -> tuple[np.ndarray, np.ndarray]: + def extract_roi(self, img: np.ndarray, + bottom: int = ROI_BOTTOM, + top: int = ROI_TOP, + left: int = ROI_LEFT, + right: int = ROI_RIGHT) -> tuple[np.ndarray, np.ndarray]: """Extract the Region of Interest Extracts the region of interest and transforms it to fit the original diff --git a/src/Active-Lane-Keeping-Assistant/world.py b/src/Active-Lane-Keeping-Assistant/world.py index 7f106f10ca..37b7d25488 100644 --- a/src/Active-Lane-Keeping-Assistant/world.py +++ b/src/Active-Lane-Keeping-Assistant/world.py @@ -47,12 +47,23 @@ def __init__(self, server_ip: str = '127.0.0.1', port: int = 2000, # Use Tesla Model 3 as Car self.bp = self.blueprint_library.filter('model3')[0] - # === 修复 2: 随机选择出生点,防止车位被占导致无法生成 === - spawn_points = self.world.get_map().get_spawn_points() - if spawn_points: - self.spawn_point = random.choice(spawn_points) - else: - self.spawn_point = carla.Transform() # 防止没有生成点的情况 + # === 修改: 将出生点设置为当前上帝视角相机的位置 === + # 1. 获取上帝视角的观察者对象 + spectator = self.world.get_spectator() + + # 2. 获取观察者现在的坐标和朝向 + transform = spectator.get_transform() + + # 3.稍微把位置抬高一点(+2米),防止相机贴地导致车卡在地里 + transform.location.z += 2.0 + + # 4. 稍微把位置往前挪一点(+5米),让你能直接看到车出现在眼前 + # (利用朝向向量计算前方的位置) + forward_vector = transform.get_forward_vector() + transform.location.x += forward_vector.x * 5.0 + transform.location.y += forward_vector.y * 5.0 + + self.spawn_point = transform # ==================================================== self.vehicle = None @@ -90,16 +101,7 @@ def reset(self) -> tuple[float, float, np.ndarray, bool]: self.vehicle = self.world.spawn_actor(self.bp, self.spawn_point) # Spawn camera - blueprint = self.blueprint_library.find('sensor.camera.rgb') - blueprint.set_attribute('image_size_x', f'{self.image_width}') - blueprint.set_attribute('image_size_y', f'{self.image_height}') - blueprint.set_attribute('fov', f'{self.fov}') - - # Relative location to car - spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7)) - self.sensor = self.world.spawn_actor(blueprint, spawn_point, - attach_to=self.vehicle) - self.sensor.listen(self.image_queue.put) + self._spawn_camera() # Spawn collision sensor blueprint = self.world.get_blueprint_library() \ @@ -110,8 +112,20 @@ def reset(self) -> tuple[float, float, np.ndarray, bool]: self.collision_sensor.listen(lambda e: {self._set_collision()}) self.initialized = True - return self._change_to_left_lane() + return 0.0, 0.0, np.zeros((self.image_height, self.image_width, 3)), False + + def _spawn_camera(self) -> None: + """Helper method to spawn the camera sensor""" + blueprint = self.blueprint_library.find('sensor.camera.rgb') + blueprint.set_attribute('image_size_x', f'{self.image_width}') + blueprint.set_attribute('image_size_y', f'{self.image_height}') + blueprint.set_attribute('fov', f'{self.fov}') + # Relative location to car + spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.sensor = self.world.spawn_actor(blueprint, spawn_point, + attach_to=self.vehicle) + self.sensor.listen(self.image_queue.put) def _set_collision(self) -> None: """Helper to set collision_detected to True. """ @@ -189,8 +203,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