diff --git a/src/autonomous_driving_car/CAS/v4.py b/src/autonomous_driving_car/CAS/v4.py new file mode 100644 index 0000000000..3fbfcf65c4 --- /dev/null +++ b/src/autonomous_driving_car/CAS/v4.py @@ -0,0 +1,338 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +CARLA 弯道自适应转向:根据弯道大小动态调整转弯幅度 +""" + +import sys +import os +import carla +import numpy as np +import math +import pygame +import traceback + +# ===================== 全局配置(动态参数)===================== +# CARLA连接 +CARLA_HOST = "localhost" +CARLA_PORT = 2000 +CARLA_TIMEOUT = 10.0 + +# 车辆配置 +VEHICLE_MODEL = "vehicle.tesla.model3" +VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距(米) +VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移(正数) + +# 转向控制(动态参数基准) +LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离 +LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离 +STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益 +STEER_GAIN_CURVE = 1.0 # 急弯转向增益 +STEER_DEADZONE = 0.05 # 转向死区 +STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数 +STEER_DIR_COEFF = 1.0 # 转向方向校准 +MAX_STEER = 1.0 # 最大转向角 + +# 弯道等级划分(方向变化量阈值) +DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值 +DIR_CHANGE_SHARP = 0.08 # 急弯阈值 + +# 速度控制 +BASE_SPEED = 28.0 # 直道基础速度 +CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数(更明显的速度衰减) +PID_KP = 0.15 +PID_KI = 0.01 +PID_KD = 0.01 + +# 相机 +CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0)) +CAMERA_WIDTH = 800 +CAMERA_HEIGHT = 600 +CAMERA_FOV = 90 + +# ===================== 纯追踪控制器(动态适配)===================== +class AdaptivePurePursuit: + def __init__(self, wheelbase): + self.wheelbase = wheelbase + self.last_steer = 0.0 # 上一帧转向角 + self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离 + + def calculate_steer(self, vehicle_transform, target_point, dir_change): + """ + 自适应纯追踪计算(根据弯道大小调整参数) + :param vehicle_transform: 车辆变换 + :param target_point: 目标点(carla.Location) + :param dir_change: 方向变化量(弯道大小) + :return: 自适应后的转向角 + """ + # 1. 获取车辆后轴位置 + forward_vec = vehicle_transform.get_forward_vector() + rear_axle_loc = carla.Location( + x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET, + y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET, + z=vehicle_transform.location.z + ) + + # 2. 转换到车辆坐标系 + dx = target_point.x - rear_axle_loc.x + dy = target_point.y - rear_axle_loc.y + yaw = math.radians(vehicle_transform.rotation.yaw) + + dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw) + dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw) + + # 3. 动态计算转向增益(根据弯道大小线性插值) + # 方向变化量越大,增益越高 + steer_gain = np.interp( + dir_change, + [0, DIR_CHANGE_SHARP], + [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE] + ) + steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE) + + # 4. 纯追踪核心计算 + if dx_vehicle < 0.1: + steer = self.last_steer + else: + # 纯追踪公式 + 弯道大小系数(dir_change*2 放大差异) + steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2) + steer = steer_rad / math.pi + + # 应用增益和方向校准 + steer *= steer_gain * STEER_DIR_COEFF + + # 5. 应用死区 + if abs(steer) < STEER_DEADZONE: + steer = 0.0 + + # 6. 低通滤波(平滑) + steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer + + # 7. 限制范围 + steer = np.clip(steer, -MAX_STEER, MAX_STEER) + + # 8. 更新状态 + self.last_steer = steer + + return steer + + def get_adaptive_lookahead(self, dir_change): + """ + 动态获取预瞄距离(根据弯道大小线性插值) + :param dir_change: 方向变化量 + :return: 自适应预瞄距离 + """ + lookahead_dist = np.interp( + dir_change, + [0, DIR_CHANGE_SHARP], + [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE] + ) + lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT) + self.last_lookahead = lookahead_dist + return lookahead_dist + +# ===================== 速度控制器(动态调整)===================== +class SpeedController: + def __init__(self, kp, ki, kd): + self.kp = kp + self.ki = ki + self.kd = kd + self.last_error = 0.0 + self.integral = 0.0 + + def calculate(self, target_speed, current_speed): + """PID速度控制""" + error = target_speed - current_speed + + p = self.kp * error + self.integral += self.ki * error + self.integral = np.clip(self.integral, -1.0, 1.0) + i = self.integral + d = self.kd * (error - self.last_error) + self.last_error = error + + output = p + i + d + return np.clip(output, 0.0, 1.0) + +# ===================== 辅助函数:计算方向变化(替代曲率)===================== +def calculate_dir_change(current_wp): + """ + 计算相邻Waypoint的方向变化量(更精细的计算,增加采样点) + :param current_wp: 当前Waypoint + :return: 方向变化的绝对值之和,弯道等级(0=直道,1=缓弯,2=急弯) + """ + waypoints = [current_wp] + # 增加采样点到5个,更准确的判断弯道大小 + for i in range(5): + next_wps = waypoints[-1].next(1.0) + if next_wps: + waypoints.append(next_wps[0]) + else: + break + + if len(waypoints) < 4: + return 0.0, 0 + + # 计算每个相邻点的方向 + dirs = [] + for i in range(1, len(waypoints)): + wp_prev = waypoints[i-1] + wp_curr = waypoints[i] + dir_rad = math.atan2( + wp_curr.transform.location.y - wp_prev.transform.location.y, + wp_curr.transform.location.x - wp_prev.transform.location.x + ) + dirs.append(dir_rad) + + # 计算方向变化的绝对值之和(放大差异) + dir_change = 0.0 + for i in range(1, len(dirs)): + dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异 + + # 划分弯道等级 + if dir_change < DIR_CHANGE_GENTLE: + curve_level = 0 # 直道 + elif dir_change < DIR_CHANGE_SHARP: + curve_level = 1 # 缓弯 + else: + curve_level = 2 # 急弯 + + return dir_change, curve_level + +# ===================== 相机管理器 ===================== +class CameraManager: + def __init__(self, world, vehicle, display): + self.world = world + self.vehicle = vehicle + self.display = display + self.camera = None + self._create_camera() + + def _create_camera(self): + bp = self.world.get_blueprint_library().find("sensor.camera.rgb") + bp.set_attribute("image_size_x", str(CAMERA_WIDTH)) + bp.set_attribute("image_size_y", str(CAMERA_HEIGHT)) + bp.set_attribute("fov", str(CAMERA_FOV)) + self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle) + self.camera.listen(self._on_image) + + def _on_image(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3] + array = array[:, :, ::-1].swapaxes(0, 1) + self.display.blit(pygame.surfarray.make_surface(array), (0, 0)) + pygame.display.flip() + + def destroy(self): + if self.camera: + self.camera.stop() + self.camera.destroy() + +# ===================== 主函数(核心逻辑)===================== +def main(): + pygame.init() + display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT)) + pygame.display.set_caption("CARLA 弯道自适应转向(最终版)") + + client = None + world = None + vehicle = None + camera_manager = None + pp_controller = None + speed_controller = None + + try: + # 1. 连接CARLA并初始化 + client = carla.Client(CARLA_HOST, CARLA_PORT) + client.set_timeout(CARLA_TIMEOUT) + world = client.get_world() + map = world.get_map() + + # 清理现有车辆 + for actor in world.get_actors().filter("vehicle.*"): + actor.destroy() + + # 生成车辆 + vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL) + spawn_points = map.get_spawn_points() + vehicle = world.spawn_actor(vehicle_bp, spawn_points[0]) + print(f"车辆生成成功:{vehicle.type_id}") + + # 初始化控制器 + pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE) + speed_controller = SpeedController(PID_KP, PID_KI, PID_KD) + camera_manager = CameraManager(world, vehicle, display) + + # 2. 主循环 + print("仿真启动,按ESC退出...") + clock = pygame.time.Clock() + running = True + + while running: + # 事件处理 + for event in pygame.event.get(): + if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): + running = False + + # 3. 获取关键数据 + vehicle_transform = vehicle.get_transform() + vehicle_vel = vehicle.get_velocity() + current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h + + # 获取当前Waypoint + current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True) + + # 计算方向变化量和弯道等级 + dir_change, curve_level = calculate_dir_change(current_wp) + + # 动态获取预瞄距离 + lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change) + + # 获取目标Waypoint(动态预瞄距离) + target_wps = current_wp.next(lookahead_dist) + target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location + + # 4. 计算目标速度(根据弯道等级调整) + curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数 + speed_factor = curve_speed_factors[min(curve_level, 2)] + target_speed = BASE_SPEED * speed_factor + target_speed = max(10.0, target_speed) + + # 5. 计算转向角(传入方向变化量,实现自适应) + steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change) + + # 6. 计算油门/刹车 + throttle = speed_controller.calculate(target_speed, current_speed) + brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0 + + # 7. 应用控制 + control = carla.VehicleControl() + control.steer = steer + control.throttle = throttle + control.brake = brake + control.hand_brake = False + vehicle.apply_control(control) + + # 8. 打印状态(调试用,显示弯道等级和动态参数) + curve_names = ["直道", "缓弯", "急弯"] + print(f"速度:{current_speed:.1f}km/h | 目标速度:{target_speed:.1f}km/h | 弯道:{curve_names[curve_level]} | 预瞄:{lookahead_dist:.1f}m | 转向:{steer:.3f}") + + # 控制帧率 + clock.tick(30) + + except Exception as e: + print(f"错误:{e}") + traceback.print_exc() + + finally: + # 清理资源 + print("清理资源...") + if camera_manager: + camera_manager.destroy() + if vehicle: + vehicle.destroy() + pygame.quit() + print("仿真结束") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py index 10a4076619..3fbfcf65c4 100644 --- a/src/autonomous_driving_car/main.py +++ b/src/autonomous_driving_car/main.py @@ -1,368 +1,338 @@ #!/usr/bin/env python +# -*- coding: utf-8 -*- """ -CARLA Basic Vehicle Spawn and Spectator Setup (sd_1/__main__.py) -完全适配 CARLA 0.9.15 版本(无任何天气预设依赖) - -This script connects to a CARLA simulator instance, removes any -pre-existing vehicles with the role 'my_car', spawns a new -Tesla Model 3 at a default spawn point, and positions the -spectator camera behind the newly spawned vehicle. -新增功能:定时循环切换CARLA模拟器的天气(晴天、多云、雨天、雾天、日落等) - -The script keeps the simulation running until interrupted (Ctrl+C), -but the vehicle does not move. +CARLA 弯道自适应转向:根据弯道大小动态调整转弯幅度 """ -# 导入CARLA模拟器的Python API +import sys +import os import carla -# 导入时间模块,用于延时和计时 -import time - - -def remove_previous_vehicle(world: carla.World) -> None: - """ - 查找并销毁所有角色名为'my_car'的车辆Actor,避免重复生成导致冲突 - - Args: - world (carla.World): CARLA模拟器的世界对象,用于获取当前所有Actor - """ - print("Searching for previous 'my_car' vehicles...") - # 过滤出所有车辆类型的Actor(vehicle.* 匹配所有车辆蓝图) - actors = world.get_actors().filter('vehicle.*') - # 记录成功销毁的车辆数量 - removed_count = 0 - - for actor in actors: - # 检查Actor的角色名是否为'my_car' - if actor.attributes.get('role_name') == 'my_car': - print(f" - Removing previous vehicle: {actor.type_id} (ID {actor.id})") - # 销毁Actor,返回布尔值表示是否成功 - if actor.destroy(): - removed_count += 1 - else: - print(f" - Failed to remove vehicle {actor.id}") - - print(f"Removed {removed_count} previous vehicles.") - - -def set_spectator_behind_vehicle(world: carla.World, vehicle: carla.Vehicle) -> None: - """ - 将旁观者相机(spectator)定位到指定车辆的后上方,实现跟随视角 - - Args: - world (carla.World): CARLA模拟器的世界对象 - vehicle (carla.Vehicle): 目标车辆Actor,用于获取车辆的位置和姿态 - """ - # 获取旁观者相机Actor(CARLA中全局唯一的 spectator) - spectator = world.get_spectator() - # 获取车辆的当前位姿(位置+旋转) - vehicle_transform = vehicle.get_transform() - # 获取车辆的前向向量,用于计算相机的相对偏移(保证相机始终在车辆后方) - forward_vector = vehicle_transform.get_forward_vector() - - # 计算相机偏移:向后15米,向上6米(基于车辆的前向向量,保证方向正确) - camera_offset = carla.Location( - x=-15 * forward_vector.x, - y=-15 * forward_vector.y, - z=6 - ) - # 构建旁观者相机的位姿: - # 位置 = 车辆位置 + 偏移量 - # 旋转 = 俯仰角-20°(向下看),偏航角与车辆一致,滚转角为0 - spectator_transform = carla.Transform( - vehicle_transform.location + camera_offset, - carla.Rotation( - pitch=-20, # 俯仰角,负数表示向下看 - yaw=vehicle_transform.rotation.yaw, # 偏航角与车辆一致 - roll=0 # 滚转角,保持水平 +import numpy as np +import math +import pygame +import traceback + +# ===================== 全局配置(动态参数)===================== +# CARLA连接 +CARLA_HOST = "localhost" +CARLA_PORT = 2000 +CARLA_TIMEOUT = 10.0 + +# 车辆配置 +VEHICLE_MODEL = "vehicle.tesla.model3" +VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距(米) +VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移(正数) + +# 转向控制(动态参数基准) +LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离 +LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离 +STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益 +STEER_GAIN_CURVE = 1.0 # 急弯转向增益 +STEER_DEADZONE = 0.05 # 转向死区 +STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数 +STEER_DIR_COEFF = 1.0 # 转向方向校准 +MAX_STEER = 1.0 # 最大转向角 + +# 弯道等级划分(方向变化量阈值) +DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值 +DIR_CHANGE_SHARP = 0.08 # 急弯阈值 + +# 速度控制 +BASE_SPEED = 28.0 # 直道基础速度 +CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数(更明显的速度衰减) +PID_KP = 0.15 +PID_KI = 0.01 +PID_KD = 0.01 + +# 相机 +CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0)) +CAMERA_WIDTH = 800 +CAMERA_HEIGHT = 600 +CAMERA_FOV = 90 + +# ===================== 纯追踪控制器(动态适配)===================== +class AdaptivePurePursuit: + def __init__(self, wheelbase): + self.wheelbase = wheelbase + self.last_steer = 0.0 # 上一帧转向角 + self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离 + + def calculate_steer(self, vehicle_transform, target_point, dir_change): + """ + 自适应纯追踪计算(根据弯道大小调整参数) + :param vehicle_transform: 车辆变换 + :param target_point: 目标点(carla.Location) + :param dir_change: 方向变化量(弯道大小) + :return: 自适应后的转向角 + """ + # 1. 获取车辆后轴位置 + forward_vec = vehicle_transform.get_forward_vector() + rear_axle_loc = carla.Location( + x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET, + y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET, + z=vehicle_transform.location.z ) - ) - - # 尝试设置相机位姿,增加异常处理提高鲁棒性 - try: - spectator.set_transform(spectator_transform) - print("Spectator camera positioned behind the vehicle.") - except Exception as e: - print(f"Error setting spectator transform: {e}") + # 2. 转换到车辆坐标系 + dx = target_point.x - rear_axle_loc.x + dy = target_point.y - rear_axle_loc.y + yaw = math.radians(vehicle_transform.rotation.yaw) -def get_weather_presets() -> list: - """ - 纯手动定义天气参数列表(完全不依赖CARLA预设,适配0.9.15) - 每个天气通过手动设置WeatherParameters的所有关键参数实现,确保兼容性 - - Returns: - list: 元组列表,每个元组包含(天气名称,carla.WeatherParameters对象) - """ - # 1. 晴天中午(太阳高悬、无云、无雨、无雾) - clear_noon = carla.WeatherParameters( - sun_altitude_angle=75.0, # 太阳高度角(75°=中午,天顶为90°) - sun_azimuth_angle=90.0, # 太阳方位角 - cloudiness=0.0, # 云量(0=无云) - precipitation=0.0, # 降水量(0=无雨) - precipitation_deposits=0.0, # 降水沉积(路面雨水) - wind_intensity=5.0, # 风力 - fog_density=0.0, # 雾密度(0=无雾) - fog_distance=0.0, # 雾的可见距离 - fog_falloff=1.0, # 雾的衰减率 - wetness=0.0, # 路面湿度 - scattering_intensity=0.0, # 光的散射强度 - mie_scattering_scale=0.0, # 米氏散射比例 - rayleigh_scattering_scale=0.0 # 瑞利散射比例 - ) - - # 2. 多云中午(高云量,阳光散射) - cloudy_noon = carla.WeatherParameters( - sun_altitude_angle=75.0, - sun_azimuth_angle=90.0, - cloudiness=80.0, # 云量80% - precipitation=0.0, - precipitation_deposits=0.0, - wind_intensity=10.0, - fog_density=0.0, - fog_distance=0.0, - fog_falloff=1.0, - wetness=0.0, - scattering_intensity=0.1, - mie_scattering_scale=0.1, - rayleigh_scattering_scale=0.1 - ) - - # 3. 小雨中午(少量降雨、路面微湿) - light_rain_noon = carla.WeatherParameters( - sun_altitude_angle=75.0, - sun_azimuth_angle=90.0, - cloudiness=90.0, # 云量90% - precipitation=20.0, # 降水量20%(小雨) - precipitation_deposits=5.0, # 路面雨水沉积5% - wind_intensity=15.0, - fog_density=5.0, # 轻微雾霭 - fog_distance=50.0, - fog_falloff=0.8, - wetness=0.2, # 路面湿度20% - scattering_intensity=0.2, - mie_scattering_scale=0.2, - rayleigh_scattering_scale=0.2 - ) - - # 4. 中雨中午(中等降雨、路面湿滑) - mid_rain_noon = carla.WeatherParameters( - sun_altitude_angle=75.0, - sun_azimuth_angle=90.0, - cloudiness=100.0, # 满云 - precipitation=50.0, # 降水量50%(中雨) - precipitation_deposits=20.0, # 路面雨水沉积20% - wind_intensity=20.0, - fog_density=15.0, # 雾密度15% - fog_distance=30.0, - fog_falloff=0.6, - wetness=0.5, # 路面湿度50% - scattering_intensity=0.3, - mie_scattering_scale=0.3, - rayleigh_scattering_scale=0.3 - ) - - # 5. 雾天中午(大雾、能见度低) - mist_noon = carla.WeatherParameters( - sun_altitude_angle=75.0, - sun_azimuth_angle=90.0, - cloudiness=50.0, - precipitation=0.0, - precipitation_deposits=0.0, - wind_intensity=5.0, - fog_density=30.0, # 雾密度30%(大雾) - fog_distance=10.0, # 雾的可见距离10米 - fog_falloff=0.5, - wetness=0.1, - scattering_intensity=0.4, - mie_scattering_scale=0.4, - rayleigh_scattering_scale=0.4 - ) - - # 6. 晴天日落(太阳低垂、暖色调、无云) - clear_sunset = carla.WeatherParameters( - sun_altitude_angle=15.0, # 太阳高度角15°(日落,地平线为0°) - sun_azimuth_angle=180.0, # 太阳方位角180°(西方) - cloudiness=0.0, - precipitation=0.0, - precipitation_deposits=0.0, - wind_intensity=5.0, - fog_density=0.0, - fog_distance=0.0, - fog_falloff=1.0, - wetness=0.0, - scattering_intensity=0.1, - mie_scattering_scale=0.1, - rayleigh_scattering_scale=0.1 - ) - - # 7. 潮湿路面(无雨但路面湿滑、轻微雾) - wet_road = carla.WeatherParameters( - sun_altitude_angle=75.0, - sun_azimuth_angle=90.0, - cloudiness=30.0, - precipitation=0.0, - precipitation_deposits=0.0, - wind_intensity=10.0, - fog_density=5.0, - fog_distance=40.0, - fog_falloff=0.9, - wetness=0.8, # 路面湿度80%(湿滑) - scattering_intensity=0.1, - mie_scattering_scale=0.1, - rayleigh_scattering_scale=0.1 - ) - - # 组合天气预设列表 - weather_presets = [ - ("Clear Noon", clear_noon), - ("Cloudy Noon", cloudy_noon), - ("Light Rain Noon", light_rain_noon), - ("Mid Rain Noon", mid_rain_noon), - ("Mist Noon", mist_noon), - ("Clear Sunset", clear_sunset), - ("Wet Road Noon", wet_road) - ] - - return weather_presets - - -def switch_weather(world: carla.World, weather: carla.WeatherParameters, weather_name: str) -> None: - """ - 设置CARLA世界的天气,并打印切换信息 - - Args: - world (carla.World): CARLA模拟器的世界对象 - weather (carla.WeatherParameters): 目标天气参数对象 - weather_name (str): 天气名称,用于打印日志 - """ - try: - # 设置世界天气 - world.set_weather(weather) - print(f"\n=== Switched to weather: {weather_name} ===") - except Exception as e: - print(f"Error switching weather to {weather_name}: {e}") + dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw) + dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw) + # 3. 动态计算转向增益(根据弯道大小线性插值) + # 方向变化量越大,增益越高 + steer_gain = np.interp( + dir_change, + [0, DIR_CHANGE_SHARP], + [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE] + ) + steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE) -def main() -> None: + # 4. 纯追踪核心计算 + if dx_vehicle < 0.1: + steer = self.last_steer + else: + # 纯追踪公式 + 弯道大小系数(dir_change*2 放大差异) + steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2) + steer = steer_rad / math.pi + + # 应用增益和方向校准 + steer *= steer_gain * STEER_DIR_COEFF + + # 5. 应用死区 + if abs(steer) < STEER_DEADZONE: + steer = 0.0 + + # 6. 低通滤波(平滑) + steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer + + # 7. 限制范围 + steer = np.clip(steer, -MAX_STEER, MAX_STEER) + + # 8. 更新状态 + self.last_steer = steer + + return steer + + def get_adaptive_lookahead(self, dir_change): + """ + 动态获取预瞄距离(根据弯道大小线性插值) + :param dir_change: 方向变化量 + :return: 自适应预瞄距离 + """ + lookahead_dist = np.interp( + dir_change, + [0, DIR_CHANGE_SHARP], + [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE] + ) + lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT) + self.last_lookahead = lookahead_dist + return lookahead_dist + +# ===================== 速度控制器(动态调整)===================== +class SpeedController: + def __init__(self, kp, ki, kd): + self.kp = kp + self.ki = ki + self.kd = kd + self.last_error = 0.0 + self.integral = 0.0 + + def calculate(self, target_speed, current_speed): + """PID速度控制""" + error = target_speed - current_speed + + p = self.kp * error + self.integral += self.ki * error + self.integral = np.clip(self.integral, -1.0, 1.0) + i = self.integral + d = self.kd * (error - self.last_error) + self.last_error = error + + output = p + i + d + return np.clip(output, 0.0, 1.0) + +# ===================== 辅助函数:计算方向变化(替代曲率)===================== +def calculate_dir_change(current_wp): """ - 主执行函数: - 1. 连接CARLA服务器 - 2. 清理旧车辆 - 3. 生成特斯拉Model3车辆 - 4. 设置旁观者相机 - 5. 定时循环切换天气 - 6. 保持仿真运行直到用户中断 + 计算相邻Waypoint的方向变化量(更精细的计算,增加采样点) + :param current_wp: 当前Waypoint + :return: 方向变化的绝对值之和,弯道等级(0=直道,1=缓弯,2=急弯) """ - # 初始化变量,避免finally块中引用未定义的变量 - client: carla.Client = None - world: carla.World = None - vehicle: carla.Vehicle = None - - # 天气相关变量初始化 - weather_presets = get_weather_presets() # 获取天气预设列表 - current_weather_index = 0 # 当前天气的索引 - weather_switch_interval = 10 # 天气切换间隔(秒) - last_weather_switch_time = time.time() # 上一次天气切换的时间戳 + waypoints = [current_wp] + # 增加采样点到5个,更准确的判断弯道大小 + for i in range(5): + next_wps = waypoints[-1].next(1.0) + if next_wps: + waypoints.append(next_wps[0]) + else: + break + + if len(waypoints) < 4: + return 0.0, 0 + + # 计算每个相邻点的方向 + dirs = [] + for i in range(1, len(waypoints)): + wp_prev = waypoints[i-1] + wp_curr = waypoints[i] + dir_rad = math.atan2( + wp_curr.transform.location.y - wp_prev.transform.location.y, + wp_curr.transform.location.x - wp_prev.transform.location.x + ) + dirs.append(dir_rad) + + # 计算方向变化的绝对值之和(放大差异) + dir_change = 0.0 + for i in range(1, len(dirs)): + dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异 + + # 划分弯道等级 + if dir_change < DIR_CHANGE_GENTLE: + curve_level = 0 # 直道 + elif dir_change < DIR_CHANGE_SHARP: + curve_level = 1 # 缓弯 + else: + curve_level = 2 # 急弯 + + return dir_change, curve_level + +# ===================== 相机管理器 ===================== +class CameraManager: + def __init__(self, world, vehicle, display): + self.world = world + self.vehicle = vehicle + self.display = display + self.camera = None + self._create_camera() + + def _create_camera(self): + bp = self.world.get_blueprint_library().find("sensor.camera.rgb") + bp.set_attribute("image_size_x", str(CAMERA_WIDTH)) + bp.set_attribute("image_size_y", str(CAMERA_HEIGHT)) + bp.set_attribute("fov", str(CAMERA_FOV)) + self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle) + self.camera.listen(self._on_image) + + def _on_image(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3] + array = array[:, :, ::-1].swapaxes(0, 1) + self.display.blit(pygame.surfarray.make_surface(array), (0, 0)) + pygame.display.flip() + + def destroy(self): + if self.camera: + self.camera.stop() + self.camera.destroy() + +# ===================== 主函数(核心逻辑)===================== +def main(): + pygame.init() + display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT)) + pygame.display.set_caption("CARLA 弯道自适应转向(最终版)") + + client = None + world = None + vehicle = None + camera_manager = None + pp_controller = None + speed_controller = None try: - # 连接到本地CARLA服务器(地址:localhost,端口:2000) - client = carla.Client('localhost', 2000) - # 设置连接超时时间(10秒),避免无限等待 - client.set_timeout(10.0) - print("Connecting to CARLA server...") - - # 获取当前CARLA世界对象(包含地图、Actor、天气等信息) + # 1. 连接CARLA并初始化 + client = carla.Client(CARLA_HOST, CARLA_PORT) + client.set_timeout(CARLA_TIMEOUT) world = client.get_world() - # 打印当前加载的地图名称 - print(f"Connected to world: {world.get_map().name}") - - # 清理之前运行残留的'my_car'车辆 - remove_previous_vehicle(world) - - # 获取地图的所有预设生成点(用于车辆/行人的生成) - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: - print("Error: No spawn points found on the map!") - return - # 选择第一个生成点作为车辆生成位置 - spawn_point = spawn_points[0] - - # 获取蓝图库(包含所有可生成的Actor蓝图:车辆、行人、传感器等) - vehicle_bp_library = world.get_blueprint_library() - # 过滤出特斯拉Model3的蓝图(vehicle.tesla.model3 是CARLA中该车辆的唯一标识) - vehicle_bp = vehicle_bp_library.filter('vehicle.tesla.model3')[0] - # 设置车辆的角色名,方便后续清理 - vehicle_bp.set_attribute('role_name', 'my_car') - - # 尝试生成车辆(try_spawn_actor会检查生成点是否被占用,返回None表示失败) - print("Attempting to spawn vehicle...") - vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) - - if vehicle is None: - # 生成失败(可能生成点被占用) - print(f"Error: Failed to spawn vehicle at {spawn_point.location}.") - return - - # 打印生成成功的车辆信息 - print(f"Vehicle {vehicle.type_id} (ID {vehicle.id}) spawned successfully.") - - # 等待车辆稳定(等待一次仿真tick,再加0.5秒延时) - world.wait_for_tick() - time.sleep(0.5) - - # 设置旁观者相机到车辆后上方 - set_spectator_behind_vehicle(world, vehicle) - - # 初始化天气为第一个预设 - switch_weather(world, weather_presets[0][1], weather_presets[0][0]) - - # 打印运行提示 - print(f"\nSimulation running. Vehicle is stationary.") - print(f"Weather will switch every {weather_switch_interval} seconds.") - print("Press Ctrl+C to stop.\n") - - # 主循环:保持仿真运行并定时切换天气 - while True: - # 等待仿真tick(推进仿真时间) - world.wait_for_tick() - - # 检查是否到达天气切换时间 - current_time = time.time() - if current_time - last_weather_switch_time >= weather_switch_interval: - # 切换到下一个天气(循环遍历预设列表) - current_weather_index = (current_weather_index + 1) % len(weather_presets) - weather_name, weather = weather_presets[current_weather_index] - switch_weather(world, weather, weather_name) - # 更新上一次切换时间 - last_weather_switch_time = current_time - - # 小延时,避免循环过于频繁(减少CPU占用) - time.sleep(0.1) - - except KeyboardInterrupt: - # 捕获用户Ctrl+C中断,友好退出 - print("\nScript stopped by user (Ctrl+C).") + map = world.get_map() + + # 清理现有车辆 + for actor in world.get_actors().filter("vehicle.*"): + actor.destroy() + + # 生成车辆 + vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL) + spawn_points = map.get_spawn_points() + vehicle = world.spawn_actor(vehicle_bp, spawn_points[0]) + print(f"车辆生成成功:{vehicle.type_id}") + + # 初始化控制器 + pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE) + speed_controller = SpeedController(PID_KP, PID_KI, PID_KD) + camera_manager = CameraManager(world, vehicle, display) + + # 2. 主循环 + print("仿真启动,按ESC退出...") + clock = pygame.time.Clock() + running = True + + while running: + # 事件处理 + for event in pygame.event.get(): + if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): + running = False + + # 3. 获取关键数据 + vehicle_transform = vehicle.get_transform() + vehicle_vel = vehicle.get_velocity() + current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h + + # 获取当前Waypoint + current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True) + + # 计算方向变化量和弯道等级 + dir_change, curve_level = calculate_dir_change(current_wp) + + # 动态获取预瞄距离 + lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change) + + # 获取目标Waypoint(动态预瞄距离) + target_wps = current_wp.next(lookahead_dist) + target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location + + # 4. 计算目标速度(根据弯道等级调整) + curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数 + speed_factor = curve_speed_factors[min(curve_level, 2)] + target_speed = BASE_SPEED * speed_factor + target_speed = max(10.0, target_speed) + + # 5. 计算转向角(传入方向变化量,实现自适应) + steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change) + + # 6. 计算油门/刹车 + throttle = speed_controller.calculate(target_speed, current_speed) + brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0 + + # 7. 应用控制 + control = carla.VehicleControl() + control.steer = steer + control.throttle = throttle + control.brake = brake + control.hand_brake = False + vehicle.apply_control(control) + + # 8. 打印状态(调试用,显示弯道等级和动态参数) + curve_names = ["直道", "缓弯", "急弯"] + print(f"速度:{current_speed:.1f}km/h | 目标速度:{target_speed:.1f}km/h | 弯道:{curve_names[curve_level]} | 预瞄:{lookahead_dist:.1f}m | 转向:{steer:.3f}") + + # 控制帧率 + clock.tick(30) + except Exception as e: - # 捕获其他未预期的异常,打印错误信息和堆栈跟踪 - print(f"\nAn unexpected error occurred: {e}") - import traceback + print(f"错误:{e}") traceback.print_exc() - finally: - # 资源清理:确保车辆被销毁,避免残留 - print("\nStarting resource cleanup...") - if vehicle is not None and vehicle.is_alive: - print(f"Destroying vehicle: {vehicle.type_id} (ID {vehicle.id})") - if vehicle.destroy(): - print("Vehicle destroyed successfully.") - else: - print("Vehicle destroy() returned False.") - else: - print("Vehicle was None or not alive, no destruction needed.") - - print("Simulation finished.") - -# 程序入口 -if __name__ == '__main__': + finally: + # 清理资源 + print("清理资源...") + if camera_manager: + camera_manager.destroy() + if vehicle: + vehicle.destroy() + pygame.quit() + print("仿真结束") + +if __name__ == "__main__": main() \ No newline at end of file