From e84d4f51cb8958c4b2d41cea1cdb41586e28f573 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Thu, 18 Dec 2025 19:19:44 +0800 Subject: [PATCH 01/11] =?UTF-8?q?=E6=8F=90=E4=BA=A4main.py=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/main.py | 368 +++++++++++++++++++++++++++++ 1 file changed, 368 insertions(+) create mode 100644 src/autonomous_driving_car/main.py diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py new file mode 100644 index 0000000000..10a4076619 --- /dev/null +++ b/src/autonomous_driving_car/main.py @@ -0,0 +1,368 @@ +#!/usr/bin/env python +""" +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模拟器的Python API +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 # 滚转角,保持水平 + ) + ) + + # 尝试设置相机位姿,增加异常处理提高鲁棒性 + try: + spectator.set_transform(spectator_transform) + print("Spectator camera positioned behind the vehicle.") + except Exception as e: + print(f"Error setting spectator transform: {e}") + + +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}") + + +def main() -> None: + """ + 主执行函数: + 1. 连接CARLA服务器 + 2. 清理旧车辆 + 3. 生成特斯拉Model3车辆 + 4. 设置旁观者相机 + 5. 定时循环切换天气 + 6. 保持仿真运行直到用户中断 + """ + # 初始化变量,避免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() # 上一次天气切换的时间戳 + + try: + # 连接到本地CARLA服务器(地址:localhost,端口:2000) + client = carla.Client('localhost', 2000) + # 设置连接超时时间(10秒),避免无限等待 + client.set_timeout(10.0) + print("Connecting to CARLA server...") + + # 获取当前CARLA世界对象(包含地图、Actor、天气等信息) + 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).") + except Exception as e: + # 捕获其他未预期的异常,打印错误信息和堆栈跟踪 + print(f"\nAn unexpected error occurred: {e}") + import traceback + 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__': + main() \ No newline at end of file From 938de18944447a50c53835bff100eaafbea2f9e6 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Thu, 18 Dec 2025 22:09:40 +0800 Subject: [PATCH 02/11] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E4=BA=BA=E7=A7=B0=E8=A7=86=E8=A7=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/v1.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/autonomous_driving_car/v1.py diff --git a/src/autonomous_driving_car/v1.py b/src/autonomous_driving_car/v1.py new file mode 100644 index 0000000000..e69de29bb2 From 5f2b01ea510d1b92e99d9847acbd5a102bed69a3 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Thu, 18 Dec 2025 22:27:28 +0800 Subject: [PATCH 03/11] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E8=A7=86=E8=A7=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/v1.py | 208 +++++++++++++++++++++++++++++++ 1 file changed, 208 insertions(+) diff --git a/src/autonomous_driving_car/v1.py b/src/autonomous_driving_car/v1.py index e69de29bb2..8e65a90916 100644 --- a/src/autonomous_driving_car/v1.py +++ b/src/autonomous_driving_car/v1.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python +""" +CARLA Vehicle Spawn with Pygame Display & Plotting (sd_3/__main__.py) + +This script connects to CARLA, spawns a vehicle, applies constant +forward throttle, and visualizes the simulation using: +1. PygameDisplay: Shows a rigidly attached camera view in a Pygame window. +2. Plotter: Plots the vehicle's X-coordinate and speed vs. time using Matplotlib. + +It also sets the initial spectator camera position to a fixed viewpoint. + +Dependencies: +- tools.pygame_display (Specifically the PygameDisplay class) +- tools.plotter_x (Specifically the Plotter class for X-coord/Speed plots) +""" + +import carla +import numpy as np +import time +import pygame # Pygame is implicitly needed by PygameDisplay +import math + +# Import helper classes from 'tools' directory using the specified format +from tools.pygame_display import PygameDisplay +from tools.plotter_x import Plotter + +# Global variable to store the simulation start time for plotting +simulation_start_time = 0.0 + +# --- Vehicle Data Functions --- +def get_location(vehicle): + """Returns the current location of the vehicle.""" + return vehicle.get_location() + +def get_speed_kmh(vehicle): + """Returns the current speed of the vehicle in km/h.""" + velocity_vector = vehicle.get_velocity() + # Calculate the magnitude of the velocity vector (speed in m/s) + speed_meters_per_second = np.linalg.norm([velocity_vector.x, velocity_vector.y, velocity_vector.z]) + return 3.6 * speed_meters_per_second + +# --- Simulation Management Functions --- +def remove_previous_vehicle(world): + """Finds and removes all vehicles with the role 'my_car'.""" + print("Searching for previous 'my_car' vehicles...") + actors = world.get_actors().filter('vehicle.*') + count = 0 + for actor in actors: + if actor.attributes.get('role_name') == 'my_car': + print(f" - Removing previous vehicle: {actor.type_id} (ID {actor.id})") + if actor.destroy(): + count += 1 + else: + print(f" - Failed to remove vehicle {actor.id}") + print(f"Removed {count} previous vehicles.") + + +# --- Spectator Setup --- +def set_initial_spectator_view(world): + """Sets the spectator camera to a predefined fixed position and rotation.""" + spectator = world.get_spectator() + # Define spectator transform using the desired fixed values (adjust as needed) + spectator_location = carla.Location(x=63.12, y=29.88, z=5.61) + spectator_rotation = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00) + spectator_transform = carla.Transform(spectator_location, spectator_rotation) + try: + spectator.set_transform(spectator_transform) + print(f"Initial spectator position set to: Loc=[{spectator_location.x:.2f}, {spectator_location.y:.2f}, {spectator_location.z:.2f}], Rot=[P:{spectator_rotation.pitch:.2f}, Y:{spectator_rotation.yaw:.2f}, R:{spectator_rotation.roll:.2f}]") + except Exception as e: + print(f"Error setting spectator transform: {e}") + + +def main(): + """Main execution function.""" + global simulation_start_time # Allow modification of the global variable + + client = None + world = None + vehicle = None + pygame_display = None + plotter = None + + try: + # Connect to CARLA + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + print("Connecting to CARLA server...") + world = client.get_world() + print(f"Connected to world: {world.get_map().name}") + + # Cleanup previous actors + remove_previous_vehicle(world) + + # Get spawn point + 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] # Use the first spawn point + print(f"Selected spawn point: {spawn_point.location}") + + # Get vehicle blueprint + vehicle_bp_library = world.get_blueprint_library() + vehicle_bp = vehicle_bp_library.filter('vehicle.tesla.model3')[0] + vehicle_bp.set_attribute('role_name', 'my_car') + + # Spawn vehicle + 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 at {vehicle.get_location()}.") + + # Set spectator view + set_initial_spectator_view(world) + + # --- Initialize Plotter & Pygame --- + print("Initializing Plotter...") + plotter = Plotter() # Uses tools.plotter_x + plotter.init_plot() + print("Plotter initialized.") + + print("Initializing Pygame display...") + pygame_display = PygameDisplay(world, vehicle) # Uses tools.pygame_display + print("Pygame display initialized.") + + # Record the start time for the plot's time axis + simulation_start_time = time.time() + + # --- Main Simulation Loop --- + print("Simulation running. Applying constant throttle. Press ESC or close Pygame window to stop.") + while True: + current_loop_time = time.time() # Get time at the start of the loop iteration + + # Handle Pygame events (QUIT, ESC) + if pygame_display.parse_events(): + print("Quit requested via Pygame window.") + break # Exit the loop if quit is requested + + # Wait for the next simulation tick + world.wait_for_tick() + + # --- Data Collection for Plotter --- + current_location = get_location(vehicle) + current_speed_kmh = get_speed_kmh(vehicle) + # Calculate time elapsed since simulation start + current_sim_time_sec = current_loop_time - simulation_start_time + + # --- Render the Pygame display --- + pygame_display.render() + + # --- Update Plot --- + if plotter is not None and plotter.is_initialized: + try: + # plotter_x expects time, x, current_speed, desired_speed + # We don't have a desired speed here, so pass 0 or current speed + plotter.update_plot(current_sim_time_sec, current_location.x, current_speed_kmh, 0.0) + except Exception as plot_update_e: + # Handle potential errors if the plot window was closed + print(f"Error updating plot (likely closed): {plot_update_e}") + plotter.cleanup_plot() # Attempt cleanup + plotter = None # Stop trying to update + + # --- Vehicle Control --- + # Apply constant forward throttle + control = carla.VehicleControl(throttle=0.8, steer=0.0, brake=0.0) + vehicle.apply_control(control) + + except KeyboardInterrupt: + print("\nScript interrupted by user (Ctrl+C).") + except Exception as e: + print(f"\nAn unexpected error occurred: {e}") + import traceback + traceback.print_exc() + + finally: + print("Starting resource cleanup...") + + # Destroy Pygame display first (handles its own camera cleanup) + if pygame_display is not None: + print("Destroying Pygame display...") + pygame_display.destroy() + print("Pygame display destroyed.") + + # Cleanup plotter + if plotter is not None: + # Check if it needs cleanup (might already be None if closed/error) + if plotter.is_initialized: + print("Cleaning up plotter...") + plotter.cleanup_plot() + print("Plotter cleaned up.") + + # Destroy the main vehicle + if vehicle is not None and vehicle.is_alive: + print(f"Destroying vehicle: {vehicle.type_id} (ID {vehicle.id})") + # vehicle.set_simulate_physics(False) # Optional: might help ensure clean removal + 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__': + main() From 6032ef7ce4754a2c660acde8ec5c51a43faeb8aa Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 10:37:05 +0800 Subject: [PATCH 04/11] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=BD=A6=E8=BE=86?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E4=BF=A1=E6=81=AF=E6=98=BE=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/v2.py | 257 +++++++++++++++++++++++++++++++ 1 file changed, 257 insertions(+) create mode 100644 src/autonomous_driving_car/v2.py diff --git a/src/autonomous_driving_car/v2.py b/src/autonomous_driving_car/v2.py new file mode 100644 index 0000000000..4e0a889bc9 --- /dev/null +++ b/src/autonomous_driving_car/v2.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +CARLA Vehicle Simulation with Visualization (sd_3/__main__.py) + +核心功能: +1. 连接CARLA仿真服务器,清理历史车辆 Actor +2. 生成特斯拉Model3车辆,施加恒定油门控制 +3. 基于PygameDisplay显示车辆挂载摄像头的实时画面 +4. 基于Plotter绘制车辆X坐标和速度随时间的变化曲线 +5. 设置固定的旁观者相机视角,方便观察仿真过程 + +依赖: +- tools.pygame_display.PygameDisplay:Pygame窗口显示摄像头画面 +- tools.plotter_x.Plotter:Matplotlib绘制X坐标/速度曲线 +""" + +# 加入这几行代码,解决模块导入问题 +import sys +import os + +# 获取项目根目录(carla-python-examples) +PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +# 将项目根目录加入Python搜索路径 +sys.path.append(PROJECT_ROOT) +import carla +import numpy as np +import time +import pygame +import math +import traceback +from typing import Optional, Tuple + +# 导入自定义工具类 +from tools.pygame_display import PygameDisplay +from tools.plotter_x import Plotter + +# ===================== 配置集中化(便于修改和维护)===================== +class SimConfig: + """仿真配置类:集中管理所有硬编码参数""" + # CARLA服务器连接信息 + CARLA_HOST = "localhost" + CARLA_PORT = 2000 + CARLA_TIMEOUT = 10.0 # 连接超时时间(秒) + + # 车辆配置 + VEHICLE_MODEL = "vehicle.tesla.model3" # 车辆模型 + VEHICLE_ROLE_NAME = "my_car" # 车辆角色名(用于清理历史车辆) + CONSTANT_THROTTLE = 0.8 # 恒定油门值(0-1) + STEER_ANGLE = 0.0 # 恒定转向角(0=直线行驶) + BRAKE_VALUE = 0.0 # 刹车值(0=不刹车) + + # 旁观者相机固定视角(可根据地图调整) + SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61) + SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00) + + # 绘图配置 + DESIRED_SPEED = 0.0 # 期望速度(若无速度控制,设为0) + +# ===================== 车辆状态计算工具函数 ===================== +def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float: + """ + 计算车辆当前速度(km/h) + :param vehicle: CARLA车辆Actor对象 + :return: 车辆速度(km/h) + """ + velocity = vehicle.get_velocity() + # 计算速度矢量的模(m/s),转换为km/h(×3.6) + speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z]) + return speed_mps * 3.6 + +def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location: + """ + 获取车辆当前位置 + :param vehicle: CARLA车辆Actor对象 + :return: 车辆的Location对象 + """ + return vehicle.get_location() + +# ===================== 仿真资源管理工具函数 ===================== +def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int: + """ + 清理地图中指定角色名的历史车辆 + :param world: CARLA的World对象 + :param role_name: 车辆角色名 + :return: 成功销毁的车辆数量 + """ + print(f"\n[资源清理] 搜索角色为'{role_name}'的历史车辆...") + vehicles = world.get_actors().filter("vehicle.*") + destroyed_count = 0 + + for vehicle in vehicles: + if vehicle.attributes.get("role_name") == role_name: + print(f" - 销毁历史车辆:{vehicle.type_id} (ID: {vehicle.id})") + if vehicle.destroy(): + destroyed_count += 1 + else: + print(f" - 销毁车辆{vehicle.id}失败") + + print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆") + return destroyed_count + +def set_spectator_fixed_view(world: carla.World, location: carla.Location, rotation: carla.Rotation) -> bool: + """ + 设置旁观者相机的固定视角 + :param world: CARLA的World对象 + :param location: 旁观者相机位置 + :param rotation: 旁观者相机旋转角度 + :return: 是否设置成功 + """ + try: + spectator = world.get_spectator() + spectator_transform = carla.Transform(location, rotation) + spectator.set_transform(spectator_transform) + print(f"\n[视角设置] 旁观者相机位置:({location.x:.2f}, {location.y:.2f}, {location.z:.2f})") + print(f"[视角设置] 旁观者相机旋转:(俯仰:{rotation.pitch:.2f}, 偏航:{rotation.yaw:.2f}, 翻滚:{rotation.roll:.2f})") + return True + except Exception as e: + print(f"[视角设置] 失败:{e}") + return False + +# ===================== 主仿真函数 ===================== +def main(): + """主仿真入口函数:完成所有仿真流程的初始化、运行和清理""" + # 初始化核心变量(所有资源对象初始化为None,便于后续清理) + client: Optional[carla.Client] = None + world: Optional[carla.World] = None + vehicle: Optional[carla.Vehicle] = None + pygame_display: Optional[PygameDisplay] = None + plotter: Optional[Plotter] = None + simulation_start_time: Optional[float] = None + + try: + # 1. 连接CARLA服务器 + print(f"[CARLA连接] 尝试连接 {SimConfig.CARLA_HOST}:{SimConfig.CARLA_PORT}...") + client = carla.Client(SimConfig.CARLA_HOST, SimConfig.CARLA_PORT) + client.set_timeout(SimConfig.CARLA_TIMEOUT) + world = client.get_world() + print(f"[CARLA连接] 成功连接到地图:{world.get_map().name}") + + # 2. 清理历史车辆 + clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME) + + # 3. 获取车辆生成点 + spawn_points = world.get_map().get_spawn_points() + if not spawn_points: + raise RuntimeError("[生成车辆] 地图中未找到可用的生成点!") + spawn_point = spawn_points[0] + print(f"[生成车辆] 选择生成点:({spawn_point.location.x:.2f}, {spawn_point.location.y:.2f}, {spawn_point.location.z:.2f})") + + # 4. 加载车辆蓝图并设置属性 + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0] + vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME) + print(f"[生成车辆] 加载车辆蓝图:{vehicle_bp.id}") + + # 5. 生成车辆 + vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) + if vehicle is None: + raise RuntimeError(f"[生成车辆] 在生成点{spawn_point.location}生成车辆失败!") + print(f"[生成车辆] 成功生成:{vehicle.type_id} (ID: {vehicle.id})") + + # 6. 设置旁观者相机视角 + set_spectator_fixed_view(world, SimConfig.SPECTATOR_LOCATION, SimConfig.SPECTATOR_ROTATION) + + # 7. 初始化可视化组件 + print("\n[可视化] 初始化绘图器(Plotter)...") + plotter = Plotter() + plotter.init_plot() + print("[可视化] 绘图器初始化完成") + + print("[可视化] 初始化Pygame显示窗口...") + pygame_display = PygameDisplay(world, vehicle) + print("[可视化] Pygame显示窗口初始化完成") + + # 8. 记录仿真开始时间 + simulation_start_time = time.time() + print(f"\n[仿真启动] 开始运行仿真(恒定油门:{SimConfig.CONSTANT_THROTTLE})") + print("[仿真启动] 按ESC或关闭Pygame窗口停止仿真...") + + # 9. 主仿真循环 + while True: + # 9.1 处理Pygame事件(关闭窗口、ESC键) + if pygame_display.parse_events(): + print("[仿真控制] 检测到Pygame退出请求,停止仿真...") + break + + # 9.2 等待仿真tick(同步仿真时间) + world.wait_for_tick() + + # 9.3 收集车辆状态数据 + current_time = time.time() + vehicle_location = get_vehicle_location(vehicle) + vehicle_speed = calculate_vehicle_speed_kmh(vehicle) + sim_elapsed_time = current_time - simulation_start_time # 仿真已运行时间(秒) + + # 9.4 渲染Pygame窗口(摄像头画面) + pygame_display.render() + + # 9.5 更新绘图器(X坐标、速度曲线) + if plotter and plotter.is_initialized: + try: + plotter.update_plot( + sim_elapsed_time, + vehicle_location.x, + vehicle_speed, + SimConfig.DESIRED_SPEED + ) + except Exception as e: + print(f"[绘图器] 更新失败(可能已关闭窗口):{e}") + plotter.cleanup_plot() + plotter = None + + # 9.6 车辆控制:施加恒定油门 + vehicle_control = carla.VehicleControl( + throttle=SimConfig.CONSTANT_THROTTLE, + steer=SimConfig.STEER_ANGLE, + brake=SimConfig.BRAKE_VALUE + ) + vehicle.apply_control(vehicle_control) + + # 异常处理 + except KeyboardInterrupt: + print("\n[仿真中断] 用户按下Ctrl+C,停止仿真...") + except RuntimeError as e: + print(f"\n[仿真错误] 运行时错误:{e}") + traceback.print_exc() + except Exception as e: + print(f"\n[仿真错误] 未知异常:{e}") + traceback.print_exc() + + # 资源清理(无论是否异常,都执行) + finally: + print("\n[资源清理] 开始清理仿真资源...") + + # 清理Pygame显示窗口 + if pygame_display: + print("[资源清理] 销毁Pygame显示窗口...") + pygame_display.destroy() + + # 清理绘图器 + if plotter and plotter.is_initialized: + print("[资源清理] 清理绘图器...") + plotter.cleanup_plot() + + # 销毁车辆 + if vehicle and vehicle.is_alive: + print(f"[资源清理] 销毁车辆:{vehicle.type_id} (ID: {vehicle.id})") + if vehicle.destroy(): + print("[资源清理] 车辆销毁成功") + else: + print("[资源清理] 车辆销毁失败") + + print("[资源清理] 仿真资源清理完成") + +if __name__ == "__main__": + main() \ No newline at end of file From 2218a0f225e93e4423d9514032857b43e5a1a218 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 14:39:50 +0800 Subject: [PATCH 05/11] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E4=B8=BB?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E4=B8=8E=E8=B7=AF=E5=BE=84=E8=B7=9F=E9=9A=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/v3.py | 344 +++++++++++++++++++++++++++++++ 1 file changed, 344 insertions(+) create mode 100644 src/autonomous_driving_car/v3.py diff --git a/src/autonomous_driving_car/v3.py b/src/autonomous_driving_car/v3.py new file mode 100644 index 0000000000..174d1de37d --- /dev/null +++ b/src/autonomous_driving_car/v3.py @@ -0,0 +1,344 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +CARLA Waypoint Following with Simple Speed Control & Pygame Display (sd_4/__main__.py) + +核心功能: +1. 连接CARLA仿真服务器,清理历史车辆Actor +2. 生成特斯拉Model3车辆,基于预定义航点实现路径跟随 +3. 采用简易的bang-bang速度控制器,根据当前速度与目标速度调整油门/刹车 +4. 基于PygameDisplay显示车辆挂载摄像头的实时画面 +5. 基于Plotter绘制车辆X坐标、实际速度与期望速度的时间曲线 +6. 设置固定的旁观者相机视角,方便观察仿真过程 + +依赖: +- tools.plotter_x.Plotter:Matplotlib绘制X坐标/速度曲线 +- tools.pygame_display.PygameDisplay:Pygame窗口显示摄像头画面 +""" + +# ===================== 路径处理(解决tools模块导入问题)===================== +import sys +import os +PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +sys.path.append(PROJECT_ROOT) +# ============================================================================== + +import carla +import numpy as np +import time +import traceback +from typing import Optional, List, Tuple + +# 导入自定义工具类 +from tools.plotter_x import Plotter +from tools.pygame_display import PygameDisplay + +# ===================== 配置集中化(便于修改和维护)===================== +class SimConfig: + """仿真配置类:集中管理所有硬编码参数""" + # CARLA服务器连接信息 + CARLA_HOST = "localhost" + CARLA_PORT = 2000 + CARLA_TIMEOUT = 10.0 # 连接超时时间(秒) + + # 车辆配置 + VEHICLE_MODEL = "vehicle.tesla.model3" + VEHICLE_ROLE_NAME = "my_car" + + # 航点配置:[x坐标, y坐标, 期望速度(km/h)] + WAYPOINTS: List[List[float]] = [ + [-64.0, 24.5, 40.0], # 起始点(第一段路径的参考点) + [40.0, 24.5, 40.0], # 制动点前的行驶点(保持40km/h) + [70.0, 24.5, 0.0] # 停止点(目标速度0km/h) + ] + WAYPOINT_THRESHOLD = 5.0 # 切换航点的距离阈值(米) + INITIAL_TARGET_WAYPOINT_ID = 1 # 初始目标航点索引(从1开始,0为起始点) + + # 旁观者相机固定视角 + SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61) + SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00) + +# ===================== 车辆状态计算工具函数 ===================== +def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location: + """ + 获取车辆当前位置 + :param vehicle: CARLA车辆Actor对象 + :return: 车辆的Location对象 + """ + return vehicle.get_location() + +def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float: + """ + 计算车辆当前速度(km/h) + :param vehicle: CARLA车辆Actor对象 + :return: 车辆速度(km/h) + """ + velocity = vehicle.get_velocity() + speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z]) + return speed_mps * 3.6 + +# ===================== 仿真资源管理工具函数 ===================== +def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int: + """ + 清理地图中指定角色名的历史车辆 + :param world: CARLA的World对象 + :param role_name: 车辆角色名 + :return: 成功销毁的车辆数量 + """ + print(f"\n[资源清理] 搜索角色为'{role_name}'的历史车辆...") + vehicles = world.get_actors().filter("vehicle.*") + destroyed_count = 0 + + for vehicle in vehicles: + if vehicle.attributes.get("role_name") == role_name: + print(f" - 销毁历史车辆:{vehicle.type_id} (ID: {vehicle.id})") + if vehicle.destroy(): + destroyed_count += 1 + else: + print(f" - 销毁车辆{vehicle.id}失败") + + print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆") + return destroyed_count + +def set_spectator_fixed_view(world: carla.World) -> bool: + """ + 设置旁观者相机的固定视角 + :param world: CARLA的World对象 + :return: 是否设置成功 + """ + try: + spectator = world.get_spectator() + spectator_transform = carla.Transform( + SimConfig.SPECTATOR_LOCATION, + SimConfig.SPECTATOR_ROTATION + ) + spectator.set_transform(spectator_transform) + print(f"\n[视角设置] 旁观者相机位置:({SimConfig.SPECTATOR_LOCATION.x:.2f}, {SimConfig.SPECTATOR_LOCATION.y:.2f}, {SimConfig.SPECTATOR_LOCATION.z:.2f})") + print(f"[视角设置] 旁观者相机旋转:(俯仰:{SimConfig.SPECTATOR_ROTATION.pitch:.2f}, 偏航:{SimConfig.SPECTATOR_ROTATION.yaw:.2f}, 翻滚:{SimConfig.SPECTATOR_ROTATION.roll:.2f})") + return True + except Exception as e: + print(f"[视角设置] 失败:{e}") + return False + +# ===================== 航点管理函数 ===================== +def update_target_waypoint( + vehicle_location: carla.Location, + current_target_id: int, + waypoints: List[List[float]], + threshold: float +) -> int: + """ + 检查是否到达当前目标航点,若到达则切换到下一个航点 + :param vehicle_location: 车辆当前位置 + :param current_target_id: 当前目标航点索引 + :param waypoints: 航点列表 + :param threshold: 切换航点的距离阈值 + :return: 更新后的目标航点索引 + """ + # 若已是最后一个航点,不再切换 + if current_target_id >= len(waypoints) - 1: + return current_target_id + + # 获取当前目标航点的位置 + target_wp_data = waypoints[current_target_id] + target_loc = carla.Location(x=target_wp_data[0], y=target_wp_data[1]) + + # 关键修改:手动计算2D距离(替代CARLA低版本不存在的distance_2d方法) + dx = vehicle_location.x - target_loc.x + dy = vehicle_location.y - target_loc.y + distance = np.sqrt(dx ** 2 + dy ** 2) + + # 若距离小于阈值,切换到下一个航点 + if distance < threshold: + print(f"[航点更新] 到达航点{current_target_id}(距离:{distance:.1f}m),新目标航点:{current_target_id + 1}") + return current_target_id + 1 + + return current_target_id + +# ===================== 速度控制器 ===================== +def simple_speed_controller(v_desired: float, v_current: float) -> carla.VehicleControl: + """ + 简易的3状态速度控制器(bang-bang控制) + 根据当前速度与期望速度的差值,全量施加油门或刹车 + :param v_desired: 期望速度(km/h) + :param v_current: 当前速度(km/h) + :return: CARLA车辆控制指令 + """ + control = carla.VehicleControl() + control.steer = 0.0 # 暂不控制转向 + control.throttle = 0.0 + control.brake = 0.0 + + # 目标速度为0时,全力刹车 + if v_desired == 0: + control.brake = 1.0 + # 当前速度小于期望速度,全力加速 + elif v_current < v_desired: + control.throttle = 1.0 + # 当前速度大于期望速度,全力刹车 + elif v_current > v_desired: + control.brake = 1.0 + + return control + +# ===================== 主仿真函数 ===================== +def main(): + """主仿真入口函数:完成所有仿真流程的初始化、运行和清理""" + # 初始化核心变量 + client: Optional[carla.Client] = None + world: Optional[carla.World] = None + vehicle: Optional[carla.Vehicle] = None + plotter: Optional[Plotter] = None + pygame_display: Optional[PygameDisplay] = None + simulation_start_time: Optional[float] = None + target_waypoint_id: int = SimConfig.INITIAL_TARGET_WAYPOINT_ID + + try: + # 1. 连接CARLA服务器 + print(f"[CARLA连接] 尝试连接 {SimConfig.CARLA_HOST}:{SimConfig.CARLA_PORT}...") + client = carla.Client(SimConfig.CARLA_HOST, SimConfig.CARLA_PORT) + client.set_timeout(SimConfig.CARLA_TIMEOUT) + world = client.get_world() + print(f"[CARLA连接] 成功连接到地图:{world.get_map().name}") + + # 2. 清理历史车辆 + clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME) + + # 3. 生成车辆(基于第一个航点作为起始位置) + map_spawn_points = world.get_map().get_spawn_points() + if not map_spawn_points: + raise RuntimeError("[生成车辆] 地图中未找到可用的生成点!") + + # 获取起始航点的位置 + start_waypoint = SimConfig.WAYPOINTS[0] + # 关键修改:将z轴高度从0.5提高到1.5,避免与地面碰撞 + start_location = carla.Location(x=start_waypoint[0], y=start_waypoint[1], z=1.5) + + # 找到距离起始位置最近的地图生成点(用于获取道路朝向) + spawn_point = min(map_spawn_points, key=lambda sp: sp.location.distance(start_location)) + spawn_point.location = start_location # 覆盖为自定义起始位置 + + print(f"[生成车辆] 目标生成位置:{start_location},使用地图朝向:{spawn_point.rotation}") + + # 加载车辆蓝图 + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0] + vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME) + + # 生成车辆 + print("[生成车辆] 尝试生成车辆...") + vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) + + # 关键修改:添加重试逻辑,第一个点失败则尝试第二个地图默认点 + if vehicle is None: + print("[生成车辆] 第一个点生成失败,尝试第二个地图默认点...") + spawn_point = map_spawn_points[1] + vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) + if vehicle is None: + raise RuntimeError(f"[生成车辆] 生成失败,请检查坐标是否合法!") + + print(f"[生成车辆] 成功生成:{vehicle.type_id} (ID: {vehicle.id})") + + # 4. 设置旁观者相机视角 + set_spectator_fixed_view(world) + + # 5. 初始化可视化组件 + print("\n[可视化] 初始化绘图器(Plotter)...") + plotter = Plotter() + plotter.init_plot() + print("[可视化] 绘图器初始化完成") + + print("[可视化] 初始化Pygame显示窗口...") + pygame_display = PygameDisplay(world, vehicle) + print("[可视化] Pygame显示窗口初始化完成") + + # 6. 记录仿真开始时间 + simulation_start_time = time.time() + target_waypoint_id = SimConfig.INITIAL_TARGET_WAYPOINT_ID + + # 7. 主仿真循环 + print("\n[仿真启动] 开始航点跟随与速度控制。按ESC或关闭Pygame窗口停止仿真...") + while True: + current_loop_time = time.time() + + # 处理Pygame事件(关闭窗口、ESC键) + if pygame_display.parse_events(): + print("[仿真控制] 检测到Pygame退出请求,停止仿真...") + break + + # 等待仿真tick(同步仿真时间) + world.wait_for_tick() + + # 8. 收集车辆状态数据 + vehicle_location = get_vehicle_location(vehicle) + vehicle_speed = calculate_vehicle_speed_kmh(vehicle) + sim_elapsed_time = current_loop_time - simulation_start_time + + # 9. 更新可视化 + # 更新绘图器 + if plotter and plotter.is_initialized: + current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] + try: + plotter.update_plot(sim_elapsed_time, vehicle_location.x, vehicle_speed, current_desired_speed) + except Exception as e: + print(f"[绘图器] 更新失败(可能已关闭窗口):{e}") + plotter.cleanup_plot() + plotter = None + + # 渲染Pygame窗口 + pygame_display.render() + + # 10. 航点跟随逻辑 + # 更新目标航点 + target_waypoint_id = update_target_waypoint( + vehicle_location, + target_waypoint_id, + SimConfig.WAYPOINTS, + SimConfig.WAYPOINT_THRESHOLD + ) + + # 获取当前目标航点的期望速度 + current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] if 0 <= target_waypoint_id < len(SimConfig.WAYPOINTS) else 0.0 + + # 计算车辆控制指令 + control = simple_speed_controller(current_desired_speed, vehicle_speed) + + # 应用控制指令 + vehicle.apply_control(control) + + # 异常处理 + except KeyboardInterrupt: + print("\n[仿真中断] 用户按下Ctrl+C,停止仿真...") + except RuntimeError as e: + print(f"\n[仿真错误] 运行时错误:{e}") + traceback.print_exc() + except Exception as e: + print(f"\n[仿真错误] 未知异常:{e}") + traceback.print_exc() + + # 资源清理 + finally: + print("\n[资源清理] 开始清理仿真资源...") + + # 清理Pygame显示窗口 + if pygame_display: + print("[资源清理] 销毁Pygame显示窗口...") + pygame_display.destroy() + + # 清理绘图器 + if plotter and plotter.is_initialized: + print("[资源清理] 清理绘图器...") + plotter.cleanup_plot() + + # 销毁车辆 + if vehicle and vehicle.is_alive: + print(f"[资源清理] 销毁车辆:{vehicle.type_id} (ID: {vehicle.id})") + if vehicle.destroy(): + print("[资源清理] 车辆销毁成功") + else: + print("[资源清理] 车辆销毁失败") + + print("[资源清理] 仿真资源清理完成") + +if __name__ == "__main__": + main() \ No newline at end of file From b1a8426883cbeb93406fe2603418e48541f97ee9 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 16:05:15 +0800 Subject: [PATCH 06/11] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E4=BA=86=E5=A4=9A?= =?UTF-8?q?=E4=B8=AA=E5=85=A5=E5=8F=A3=E8=84=9A=E6=9C=AC=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/{ => FPV}/v1.py | 0 src/autonomous_driving_car/{ => MCP}/v2.py | 0 src/autonomous_driving_car/v3.py | 344 --------------------- 3 files changed, 344 deletions(-) rename src/autonomous_driving_car/{ => FPV}/v1.py (100%) rename src/autonomous_driving_car/{ => MCP}/v2.py (100%) delete mode 100644 src/autonomous_driving_car/v3.py diff --git a/src/autonomous_driving_car/v1.py b/src/autonomous_driving_car/FPV/v1.py similarity index 100% rename from src/autonomous_driving_car/v1.py rename to src/autonomous_driving_car/FPV/v1.py diff --git a/src/autonomous_driving_car/v2.py b/src/autonomous_driving_car/MCP/v2.py similarity index 100% rename from src/autonomous_driving_car/v2.py rename to src/autonomous_driving_car/MCP/v2.py diff --git a/src/autonomous_driving_car/v3.py b/src/autonomous_driving_car/v3.py deleted file mode 100644 index 174d1de37d..0000000000 --- a/src/autonomous_driving_car/v3.py +++ /dev/null @@ -1,344 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -""" -CARLA Waypoint Following with Simple Speed Control & Pygame Display (sd_4/__main__.py) - -核心功能: -1. 连接CARLA仿真服务器,清理历史车辆Actor -2. 生成特斯拉Model3车辆,基于预定义航点实现路径跟随 -3. 采用简易的bang-bang速度控制器,根据当前速度与目标速度调整油门/刹车 -4. 基于PygameDisplay显示车辆挂载摄像头的实时画面 -5. 基于Plotter绘制车辆X坐标、实际速度与期望速度的时间曲线 -6. 设置固定的旁观者相机视角,方便观察仿真过程 - -依赖: -- tools.plotter_x.Plotter:Matplotlib绘制X坐标/速度曲线 -- tools.pygame_display.PygameDisplay:Pygame窗口显示摄像头画面 -""" - -# ===================== 路径处理(解决tools模块导入问题)===================== -import sys -import os -PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) -sys.path.append(PROJECT_ROOT) -# ============================================================================== - -import carla -import numpy as np -import time -import traceback -from typing import Optional, List, Tuple - -# 导入自定义工具类 -from tools.plotter_x import Plotter -from tools.pygame_display import PygameDisplay - -# ===================== 配置集中化(便于修改和维护)===================== -class SimConfig: - """仿真配置类:集中管理所有硬编码参数""" - # CARLA服务器连接信息 - CARLA_HOST = "localhost" - CARLA_PORT = 2000 - CARLA_TIMEOUT = 10.0 # 连接超时时间(秒) - - # 车辆配置 - VEHICLE_MODEL = "vehicle.tesla.model3" - VEHICLE_ROLE_NAME = "my_car" - - # 航点配置:[x坐标, y坐标, 期望速度(km/h)] - WAYPOINTS: List[List[float]] = [ - [-64.0, 24.5, 40.0], # 起始点(第一段路径的参考点) - [40.0, 24.5, 40.0], # 制动点前的行驶点(保持40km/h) - [70.0, 24.5, 0.0] # 停止点(目标速度0km/h) - ] - WAYPOINT_THRESHOLD = 5.0 # 切换航点的距离阈值(米) - INITIAL_TARGET_WAYPOINT_ID = 1 # 初始目标航点索引(从1开始,0为起始点) - - # 旁观者相机固定视角 - SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61) - SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00) - -# ===================== 车辆状态计算工具函数 ===================== -def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location: - """ - 获取车辆当前位置 - :param vehicle: CARLA车辆Actor对象 - :return: 车辆的Location对象 - """ - return vehicle.get_location() - -def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float: - """ - 计算车辆当前速度(km/h) - :param vehicle: CARLA车辆Actor对象 - :return: 车辆速度(km/h) - """ - velocity = vehicle.get_velocity() - speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z]) - return speed_mps * 3.6 - -# ===================== 仿真资源管理工具函数 ===================== -def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int: - """ - 清理地图中指定角色名的历史车辆 - :param world: CARLA的World对象 - :param role_name: 车辆角色名 - :return: 成功销毁的车辆数量 - """ - print(f"\n[资源清理] 搜索角色为'{role_name}'的历史车辆...") - vehicles = world.get_actors().filter("vehicle.*") - destroyed_count = 0 - - for vehicle in vehicles: - if vehicle.attributes.get("role_name") == role_name: - print(f" - 销毁历史车辆:{vehicle.type_id} (ID: {vehicle.id})") - if vehicle.destroy(): - destroyed_count += 1 - else: - print(f" - 销毁车辆{vehicle.id}失败") - - print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆") - return destroyed_count - -def set_spectator_fixed_view(world: carla.World) -> bool: - """ - 设置旁观者相机的固定视角 - :param world: CARLA的World对象 - :return: 是否设置成功 - """ - try: - spectator = world.get_spectator() - spectator_transform = carla.Transform( - SimConfig.SPECTATOR_LOCATION, - SimConfig.SPECTATOR_ROTATION - ) - spectator.set_transform(spectator_transform) - print(f"\n[视角设置] 旁观者相机位置:({SimConfig.SPECTATOR_LOCATION.x:.2f}, {SimConfig.SPECTATOR_LOCATION.y:.2f}, {SimConfig.SPECTATOR_LOCATION.z:.2f})") - print(f"[视角设置] 旁观者相机旋转:(俯仰:{SimConfig.SPECTATOR_ROTATION.pitch:.2f}, 偏航:{SimConfig.SPECTATOR_ROTATION.yaw:.2f}, 翻滚:{SimConfig.SPECTATOR_ROTATION.roll:.2f})") - return True - except Exception as e: - print(f"[视角设置] 失败:{e}") - return False - -# ===================== 航点管理函数 ===================== -def update_target_waypoint( - vehicle_location: carla.Location, - current_target_id: int, - waypoints: List[List[float]], - threshold: float -) -> int: - """ - 检查是否到达当前目标航点,若到达则切换到下一个航点 - :param vehicle_location: 车辆当前位置 - :param current_target_id: 当前目标航点索引 - :param waypoints: 航点列表 - :param threshold: 切换航点的距离阈值 - :return: 更新后的目标航点索引 - """ - # 若已是最后一个航点,不再切换 - if current_target_id >= len(waypoints) - 1: - return current_target_id - - # 获取当前目标航点的位置 - target_wp_data = waypoints[current_target_id] - target_loc = carla.Location(x=target_wp_data[0], y=target_wp_data[1]) - - # 关键修改:手动计算2D距离(替代CARLA低版本不存在的distance_2d方法) - dx = vehicle_location.x - target_loc.x - dy = vehicle_location.y - target_loc.y - distance = np.sqrt(dx ** 2 + dy ** 2) - - # 若距离小于阈值,切换到下一个航点 - if distance < threshold: - print(f"[航点更新] 到达航点{current_target_id}(距离:{distance:.1f}m),新目标航点:{current_target_id + 1}") - return current_target_id + 1 - - return current_target_id - -# ===================== 速度控制器 ===================== -def simple_speed_controller(v_desired: float, v_current: float) -> carla.VehicleControl: - """ - 简易的3状态速度控制器(bang-bang控制) - 根据当前速度与期望速度的差值,全量施加油门或刹车 - :param v_desired: 期望速度(km/h) - :param v_current: 当前速度(km/h) - :return: CARLA车辆控制指令 - """ - control = carla.VehicleControl() - control.steer = 0.0 # 暂不控制转向 - control.throttle = 0.0 - control.brake = 0.0 - - # 目标速度为0时,全力刹车 - if v_desired == 0: - control.brake = 1.0 - # 当前速度小于期望速度,全力加速 - elif v_current < v_desired: - control.throttle = 1.0 - # 当前速度大于期望速度,全力刹车 - elif v_current > v_desired: - control.brake = 1.0 - - return control - -# ===================== 主仿真函数 ===================== -def main(): - """主仿真入口函数:完成所有仿真流程的初始化、运行和清理""" - # 初始化核心变量 - client: Optional[carla.Client] = None - world: Optional[carla.World] = None - vehicle: Optional[carla.Vehicle] = None - plotter: Optional[Plotter] = None - pygame_display: Optional[PygameDisplay] = None - simulation_start_time: Optional[float] = None - target_waypoint_id: int = SimConfig.INITIAL_TARGET_WAYPOINT_ID - - try: - # 1. 连接CARLA服务器 - print(f"[CARLA连接] 尝试连接 {SimConfig.CARLA_HOST}:{SimConfig.CARLA_PORT}...") - client = carla.Client(SimConfig.CARLA_HOST, SimConfig.CARLA_PORT) - client.set_timeout(SimConfig.CARLA_TIMEOUT) - world = client.get_world() - print(f"[CARLA连接] 成功连接到地图:{world.get_map().name}") - - # 2. 清理历史车辆 - clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME) - - # 3. 生成车辆(基于第一个航点作为起始位置) - map_spawn_points = world.get_map().get_spawn_points() - if not map_spawn_points: - raise RuntimeError("[生成车辆] 地图中未找到可用的生成点!") - - # 获取起始航点的位置 - start_waypoint = SimConfig.WAYPOINTS[0] - # 关键修改:将z轴高度从0.5提高到1.5,避免与地面碰撞 - start_location = carla.Location(x=start_waypoint[0], y=start_waypoint[1], z=1.5) - - # 找到距离起始位置最近的地图生成点(用于获取道路朝向) - spawn_point = min(map_spawn_points, key=lambda sp: sp.location.distance(start_location)) - spawn_point.location = start_location # 覆盖为自定义起始位置 - - print(f"[生成车辆] 目标生成位置:{start_location},使用地图朝向:{spawn_point.rotation}") - - # 加载车辆蓝图 - blueprint_library = world.get_blueprint_library() - vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0] - vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME) - - # 生成车辆 - print("[生成车辆] 尝试生成车辆...") - vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) - - # 关键修改:添加重试逻辑,第一个点失败则尝试第二个地图默认点 - if vehicle is None: - print("[生成车辆] 第一个点生成失败,尝试第二个地图默认点...") - spawn_point = map_spawn_points[1] - vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) - if vehicle is None: - raise RuntimeError(f"[生成车辆] 生成失败,请检查坐标是否合法!") - - print(f"[生成车辆] 成功生成:{vehicle.type_id} (ID: {vehicle.id})") - - # 4. 设置旁观者相机视角 - set_spectator_fixed_view(world) - - # 5. 初始化可视化组件 - print("\n[可视化] 初始化绘图器(Plotter)...") - plotter = Plotter() - plotter.init_plot() - print("[可视化] 绘图器初始化完成") - - print("[可视化] 初始化Pygame显示窗口...") - pygame_display = PygameDisplay(world, vehicle) - print("[可视化] Pygame显示窗口初始化完成") - - # 6. 记录仿真开始时间 - simulation_start_time = time.time() - target_waypoint_id = SimConfig.INITIAL_TARGET_WAYPOINT_ID - - # 7. 主仿真循环 - print("\n[仿真启动] 开始航点跟随与速度控制。按ESC或关闭Pygame窗口停止仿真...") - while True: - current_loop_time = time.time() - - # 处理Pygame事件(关闭窗口、ESC键) - if pygame_display.parse_events(): - print("[仿真控制] 检测到Pygame退出请求,停止仿真...") - break - - # 等待仿真tick(同步仿真时间) - world.wait_for_tick() - - # 8. 收集车辆状态数据 - vehicle_location = get_vehicle_location(vehicle) - vehicle_speed = calculate_vehicle_speed_kmh(vehicle) - sim_elapsed_time = current_loop_time - simulation_start_time - - # 9. 更新可视化 - # 更新绘图器 - if plotter and plotter.is_initialized: - current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] - try: - plotter.update_plot(sim_elapsed_time, vehicle_location.x, vehicle_speed, current_desired_speed) - except Exception as e: - print(f"[绘图器] 更新失败(可能已关闭窗口):{e}") - plotter.cleanup_plot() - plotter = None - - # 渲染Pygame窗口 - pygame_display.render() - - # 10. 航点跟随逻辑 - # 更新目标航点 - target_waypoint_id = update_target_waypoint( - vehicle_location, - target_waypoint_id, - SimConfig.WAYPOINTS, - SimConfig.WAYPOINT_THRESHOLD - ) - - # 获取当前目标航点的期望速度 - current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] if 0 <= target_waypoint_id < len(SimConfig.WAYPOINTS) else 0.0 - - # 计算车辆控制指令 - control = simple_speed_controller(current_desired_speed, vehicle_speed) - - # 应用控制指令 - vehicle.apply_control(control) - - # 异常处理 - except KeyboardInterrupt: - print("\n[仿真中断] 用户按下Ctrl+C,停止仿真...") - except RuntimeError as e: - print(f"\n[仿真错误] 运行时错误:{e}") - traceback.print_exc() - except Exception as e: - print(f"\n[仿真错误] 未知异常:{e}") - traceback.print_exc() - - # 资源清理 - finally: - print("\n[资源清理] 开始清理仿真资源...") - - # 清理Pygame显示窗口 - if pygame_display: - print("[资源清理] 销毁Pygame显示窗口...") - pygame_display.destroy() - - # 清理绘图器 - if plotter and plotter.is_initialized: - print("[资源清理] 清理绘图器...") - plotter.cleanup_plot() - - # 销毁车辆 - if vehicle and vehicle.is_alive: - print(f"[资源清理] 销毁车辆:{vehicle.type_id} (ID: {vehicle.id})") - if vehicle.destroy(): - print("[资源清理] 车辆销毁成功") - else: - print("[资源清理] 车辆销毁失败") - - print("[资源清理] 仿真资源清理完成") - -if __name__ == "__main__": - main() \ No newline at end of file From 9768bf171088cb8dc1faf8605835424a4c93e577 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 17:14:36 +0800 Subject: [PATCH 07/11] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E4=B8=BB?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E4=B8=8E=E8=B7=AF=E5=BE=84=E8=B7=9F=E9=9A=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/MCP/v2.py | 247 ++++++++++++++++++--------- 1 file changed, 167 insertions(+), 80 deletions(-) diff --git a/src/autonomous_driving_car/MCP/v2.py b/src/autonomous_driving_car/MCP/v2.py index 4e0a889bc9..174d1de37d 100644 --- a/src/autonomous_driving_car/MCP/v2.py +++ b/src/autonomous_driving_car/MCP/v2.py @@ -1,39 +1,37 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- """ -CARLA Vehicle Simulation with Visualization (sd_3/__main__.py) +CARLA Waypoint Following with Simple Speed Control & Pygame Display (sd_4/__main__.py) 核心功能: -1. 连接CARLA仿真服务器,清理历史车辆 Actor -2. 生成特斯拉Model3车辆,施加恒定油门控制 -3. 基于PygameDisplay显示车辆挂载摄像头的实时画面 -4. 基于Plotter绘制车辆X坐标和速度随时间的变化曲线 -5. 设置固定的旁观者相机视角,方便观察仿真过程 +1. 连接CARLA仿真服务器,清理历史车辆Actor +2. 生成特斯拉Model3车辆,基于预定义航点实现路径跟随 +3. 采用简易的bang-bang速度控制器,根据当前速度与目标速度调整油门/刹车 +4. 基于PygameDisplay显示车辆挂载摄像头的实时画面 +5. 基于Plotter绘制车辆X坐标、实际速度与期望速度的时间曲线 +6. 设置固定的旁观者相机视角,方便观察仿真过程 依赖: -- tools.pygame_display.PygameDisplay:Pygame窗口显示摄像头画面 - tools.plotter_x.Plotter:Matplotlib绘制X坐标/速度曲线 +- tools.pygame_display.PygameDisplay:Pygame窗口显示摄像头画面 """ -# 加入这几行代码,解决模块导入问题 +# ===================== 路径处理(解决tools模块导入问题)===================== import sys import os - -# 获取项目根目录(carla-python-examples) PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) -# 将项目根目录加入Python搜索路径 sys.path.append(PROJECT_ROOT) +# ============================================================================== + import carla import numpy as np import time -import pygame -import math import traceback -from typing import Optional, Tuple +from typing import Optional, List, Tuple # 导入自定义工具类 -from tools.pygame_display import PygameDisplay from tools.plotter_x import Plotter +from tools.pygame_display import PygameDisplay # ===================== 配置集中化(便于修改和维护)===================== class SimConfig: @@ -44,20 +42,31 @@ class SimConfig: CARLA_TIMEOUT = 10.0 # 连接超时时间(秒) # 车辆配置 - VEHICLE_MODEL = "vehicle.tesla.model3" # 车辆模型 - VEHICLE_ROLE_NAME = "my_car" # 车辆角色名(用于清理历史车辆) - CONSTANT_THROTTLE = 0.8 # 恒定油门值(0-1) - STEER_ANGLE = 0.0 # 恒定转向角(0=直线行驶) - BRAKE_VALUE = 0.0 # 刹车值(0=不刹车) - - # 旁观者相机固定视角(可根据地图调整) + VEHICLE_MODEL = "vehicle.tesla.model3" + VEHICLE_ROLE_NAME = "my_car" + + # 航点配置:[x坐标, y坐标, 期望速度(km/h)] + WAYPOINTS: List[List[float]] = [ + [-64.0, 24.5, 40.0], # 起始点(第一段路径的参考点) + [40.0, 24.5, 40.0], # 制动点前的行驶点(保持40km/h) + [70.0, 24.5, 0.0] # 停止点(目标速度0km/h) + ] + WAYPOINT_THRESHOLD = 5.0 # 切换航点的距离阈值(米) + INITIAL_TARGET_WAYPOINT_ID = 1 # 初始目标航点索引(从1开始,0为起始点) + + # 旁观者相机固定视角 SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61) SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00) - # 绘图配置 - DESIRED_SPEED = 0.0 # 期望速度(若无速度控制,设为0) - # ===================== 车辆状态计算工具函数 ===================== +def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location: + """ + 获取车辆当前位置 + :param vehicle: CARLA车辆Actor对象 + :return: 车辆的Location对象 + """ + return vehicle.get_location() + def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float: """ 计算车辆当前速度(km/h) @@ -65,18 +74,9 @@ def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float: :return: 车辆速度(km/h) """ velocity = vehicle.get_velocity() - # 计算速度矢量的模(m/s),转换为km/h(×3.6) speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z]) return speed_mps * 3.6 -def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location: - """ - 获取车辆当前位置 - :param vehicle: CARLA车辆Actor对象 - :return: 车辆的Location对象 - """ - return vehicle.get_location() - # ===================== 仿真资源管理工具函数 ===================== def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int: """ @@ -100,35 +100,98 @@ def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int: print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆") return destroyed_count -def set_spectator_fixed_view(world: carla.World, location: carla.Location, rotation: carla.Rotation) -> bool: +def set_spectator_fixed_view(world: carla.World) -> bool: """ 设置旁观者相机的固定视角 :param world: CARLA的World对象 - :param location: 旁观者相机位置 - :param rotation: 旁观者相机旋转角度 :return: 是否设置成功 """ try: spectator = world.get_spectator() - spectator_transform = carla.Transform(location, rotation) + spectator_transform = carla.Transform( + SimConfig.SPECTATOR_LOCATION, + SimConfig.SPECTATOR_ROTATION + ) spectator.set_transform(spectator_transform) - print(f"\n[视角设置] 旁观者相机位置:({location.x:.2f}, {location.y:.2f}, {location.z:.2f})") - print(f"[视角设置] 旁观者相机旋转:(俯仰:{rotation.pitch:.2f}, 偏航:{rotation.yaw:.2f}, 翻滚:{rotation.roll:.2f})") + print(f"\n[视角设置] 旁观者相机位置:({SimConfig.SPECTATOR_LOCATION.x:.2f}, {SimConfig.SPECTATOR_LOCATION.y:.2f}, {SimConfig.SPECTATOR_LOCATION.z:.2f})") + print(f"[视角设置] 旁观者相机旋转:(俯仰:{SimConfig.SPECTATOR_ROTATION.pitch:.2f}, 偏航:{SimConfig.SPECTATOR_ROTATION.yaw:.2f}, 翻滚:{SimConfig.SPECTATOR_ROTATION.roll:.2f})") return True except Exception as e: print(f"[视角设置] 失败:{e}") return False +# ===================== 航点管理函数 ===================== +def update_target_waypoint( + vehicle_location: carla.Location, + current_target_id: int, + waypoints: List[List[float]], + threshold: float +) -> int: + """ + 检查是否到达当前目标航点,若到达则切换到下一个航点 + :param vehicle_location: 车辆当前位置 + :param current_target_id: 当前目标航点索引 + :param waypoints: 航点列表 + :param threshold: 切换航点的距离阈值 + :return: 更新后的目标航点索引 + """ + # 若已是最后一个航点,不再切换 + if current_target_id >= len(waypoints) - 1: + return current_target_id + + # 获取当前目标航点的位置 + target_wp_data = waypoints[current_target_id] + target_loc = carla.Location(x=target_wp_data[0], y=target_wp_data[1]) + + # 关键修改:手动计算2D距离(替代CARLA低版本不存在的distance_2d方法) + dx = vehicle_location.x - target_loc.x + dy = vehicle_location.y - target_loc.y + distance = np.sqrt(dx ** 2 + dy ** 2) + + # 若距离小于阈值,切换到下一个航点 + if distance < threshold: + print(f"[航点更新] 到达航点{current_target_id}(距离:{distance:.1f}m),新目标航点:{current_target_id + 1}") + return current_target_id + 1 + + return current_target_id + +# ===================== 速度控制器 ===================== +def simple_speed_controller(v_desired: float, v_current: float) -> carla.VehicleControl: + """ + 简易的3状态速度控制器(bang-bang控制) + 根据当前速度与期望速度的差值,全量施加油门或刹车 + :param v_desired: 期望速度(km/h) + :param v_current: 当前速度(km/h) + :return: CARLA车辆控制指令 + """ + control = carla.VehicleControl() + control.steer = 0.0 # 暂不控制转向 + control.throttle = 0.0 + control.brake = 0.0 + + # 目标速度为0时,全力刹车 + if v_desired == 0: + control.brake = 1.0 + # 当前速度小于期望速度,全力加速 + elif v_current < v_desired: + control.throttle = 1.0 + # 当前速度大于期望速度,全力刹车 + elif v_current > v_desired: + control.brake = 1.0 + + return control + # ===================== 主仿真函数 ===================== def main(): """主仿真入口函数:完成所有仿真流程的初始化、运行和清理""" - # 初始化核心变量(所有资源对象初始化为None,便于后续清理) + # 初始化核心变量 client: Optional[carla.Client] = None world: Optional[carla.World] = None vehicle: Optional[carla.Vehicle] = None - pygame_display: Optional[PygameDisplay] = None plotter: Optional[Plotter] = None + pygame_display: Optional[PygameDisplay] = None simulation_start_time: Optional[float] = None + target_waypoint_id: int = SimConfig.INITIAL_TARGET_WAYPOINT_ID try: # 1. 连接CARLA服务器 @@ -141,29 +204,45 @@ def main(): # 2. 清理历史车辆 clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME) - # 3. 获取车辆生成点 - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: + # 3. 生成车辆(基于第一个航点作为起始位置) + map_spawn_points = world.get_map().get_spawn_points() + if not map_spawn_points: raise RuntimeError("[生成车辆] 地图中未找到可用的生成点!") - spawn_point = spawn_points[0] - print(f"[生成车辆] 选择生成点:({spawn_point.location.x:.2f}, {spawn_point.location.y:.2f}, {spawn_point.location.z:.2f})") - # 4. 加载车辆蓝图并设置属性 + # 获取起始航点的位置 + start_waypoint = SimConfig.WAYPOINTS[0] + # 关键修改:将z轴高度从0.5提高到1.5,避免与地面碰撞 + start_location = carla.Location(x=start_waypoint[0], y=start_waypoint[1], z=1.5) + + # 找到距离起始位置最近的地图生成点(用于获取道路朝向) + spawn_point = min(map_spawn_points, key=lambda sp: sp.location.distance(start_location)) + spawn_point.location = start_location # 覆盖为自定义起始位置 + + print(f"[生成车辆] 目标生成位置:{start_location},使用地图朝向:{spawn_point.rotation}") + + # 加载车辆蓝图 blueprint_library = world.get_blueprint_library() vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0] vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME) - print(f"[生成车辆] 加载车辆蓝图:{vehicle_bp.id}") - # 5. 生成车辆 + # 生成车辆 + print("[生成车辆] 尝试生成车辆...") vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) + + # 关键修改:添加重试逻辑,第一个点失败则尝试第二个地图默认点 if vehicle is None: - raise RuntimeError(f"[生成车辆] 在生成点{spawn_point.location}生成车辆失败!") + print("[生成车辆] 第一个点生成失败,尝试第二个地图默认点...") + spawn_point = map_spawn_points[1] + vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) + if vehicle is None: + raise RuntimeError(f"[生成车辆] 生成失败,请检查坐标是否合法!") + print(f"[生成车辆] 成功生成:{vehicle.type_id} (ID: {vehicle.id})") - # 6. 设置旁观者相机视角 - set_spectator_fixed_view(world, SimConfig.SPECTATOR_LOCATION, SimConfig.SPECTATOR_ROTATION) + # 4. 设置旁观者相机视角 + set_spectator_fixed_view(world) - # 7. 初始化可视化组件 + # 5. 初始化可视化组件 print("\n[可视化] 初始化绘图器(Plotter)...") plotter = Plotter() plotter.init_plot() @@ -173,51 +252,59 @@ def main(): pygame_display = PygameDisplay(world, vehicle) print("[可视化] Pygame显示窗口初始化完成") - # 8. 记录仿真开始时间 + # 6. 记录仿真开始时间 simulation_start_time = time.time() - print(f"\n[仿真启动] 开始运行仿真(恒定油门:{SimConfig.CONSTANT_THROTTLE})") - print("[仿真启动] 按ESC或关闭Pygame窗口停止仿真...") + target_waypoint_id = SimConfig.INITIAL_TARGET_WAYPOINT_ID - # 9. 主仿真循环 + # 7. 主仿真循环 + print("\n[仿真启动] 开始航点跟随与速度控制。按ESC或关闭Pygame窗口停止仿真...") while True: - # 9.1 处理Pygame事件(关闭窗口、ESC键) + current_loop_time = time.time() + + # 处理Pygame事件(关闭窗口、ESC键) if pygame_display.parse_events(): print("[仿真控制] 检测到Pygame退出请求,停止仿真...") break - # 9.2 等待仿真tick(同步仿真时间) + # 等待仿真tick(同步仿真时间) world.wait_for_tick() - # 9.3 收集车辆状态数据 - current_time = time.time() + # 8. 收集车辆状态数据 vehicle_location = get_vehicle_location(vehicle) vehicle_speed = calculate_vehicle_speed_kmh(vehicle) - sim_elapsed_time = current_time - simulation_start_time # 仿真已运行时间(秒) - - # 9.4 渲染Pygame窗口(摄像头画面) - pygame_display.render() + sim_elapsed_time = current_loop_time - simulation_start_time - # 9.5 更新绘图器(X坐标、速度曲线) + # 9. 更新可视化 + # 更新绘图器 if plotter and plotter.is_initialized: + current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] try: - plotter.update_plot( - sim_elapsed_time, - vehicle_location.x, - vehicle_speed, - SimConfig.DESIRED_SPEED - ) + plotter.update_plot(sim_elapsed_time, vehicle_location.x, vehicle_speed, current_desired_speed) except Exception as e: print(f"[绘图器] 更新失败(可能已关闭窗口):{e}") plotter.cleanup_plot() plotter = None - # 9.6 车辆控制:施加恒定油门 - vehicle_control = carla.VehicleControl( - throttle=SimConfig.CONSTANT_THROTTLE, - steer=SimConfig.STEER_ANGLE, - brake=SimConfig.BRAKE_VALUE + # 渲染Pygame窗口 + pygame_display.render() + + # 10. 航点跟随逻辑 + # 更新目标航点 + target_waypoint_id = update_target_waypoint( + vehicle_location, + target_waypoint_id, + SimConfig.WAYPOINTS, + SimConfig.WAYPOINT_THRESHOLD ) - vehicle.apply_control(vehicle_control) + + # 获取当前目标航点的期望速度 + current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] if 0 <= target_waypoint_id < len(SimConfig.WAYPOINTS) else 0.0 + + # 计算车辆控制指令 + control = simple_speed_controller(current_desired_speed, vehicle_speed) + + # 应用控制指令 + vehicle.apply_control(control) # 异常处理 except KeyboardInterrupt: @@ -229,7 +316,7 @@ def main(): print(f"\n[仿真错误] 未知异常:{e}") traceback.print_exc() - # 资源清理(无论是否异常,都执行) + # 资源清理 finally: print("\n[资源清理] 开始清理仿真资源...") From e4c440b21e54ce30f5e68dfdb0ce44c20cd1dea0 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 20:50:08 +0800 Subject: [PATCH 08/11] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=BC=AF=E9=81=93?= =?UTF-8?q?=E8=87=AA=E9=80=82=E5=BA=94=E8=BD=AC=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/CAS/v4.py | 338 +++++++++++++++++++++++++++ 1 file changed, 338 insertions(+) create mode 100644 src/autonomous_driving_car/CAS/v4.py 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 From 85e2c10d83d883b0c7e55c1537e46b65fd46d020 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 21:14:28 +0800 Subject: [PATCH 09/11] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=BC=AF=E9=81=93?= =?UTF-8?q?=E8=87=AA=E9=80=82=E5=BA=94=E8=BD=AC=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/main.py | 668 ++++++++++++++--------------- 1 file changed, 319 insertions(+), 349 deletions(-) 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 From 51bb9b3033b91787ac063d8885b1568d25cd0096 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Fri, 19 Dec 2025 22:02:15 +0800 Subject: [PATCH 10/11] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E8=BD=A6=E8=BE=86?= =?UTF-8?q?=E6=B2=BF=E9=81=93=E8=B7=AF=E7=9A=84=E8=87=AA=E4=B8=BB=E8=88=AA?= =?UTF-8?q?=E7=82=B9=E8=B7=9F=E8=B8=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/main.py | 855 +++++++++++++++++++---------- 1 file changed, 552 insertions(+), 303 deletions(-) diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py index 3fbfcf65c4..64f88ed002 100644 --- a/src/autonomous_driving_car/main.py +++ b/src/autonomous_driving_car/main.py @@ -1,338 +1,587 @@ #!/usr/bin/env python -# -*- coding: utf-8 -*- """ -CARLA 弯道自适应转向:根据弯道大小动态调整转弯幅度 +CARLA Waypoint Following - Dynamic Road Waypoints (sd_6/__main__.py) + +核心改进: +1. 动态生成沿道路的航点,替代固定坐标,避开障碍物 +2. 选择地图开阔区域生成车辆(Town03主干道) +3. 增加障碍物检测,确保行驶路段安全 +4. 优化航点跟踪逻辑,适配动态道路 """ +# ===================== 系统导入 & 路径配置 ===================== import sys import os import carla import numpy as np +import time import math +import logging +from typing import List, Tuple, Optional, Union 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) +import matplotlib.pyplot as plt + +# 获取当前脚本目录并加入搜索路径 +current_dir = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(current_dir) + +# ===================== PID控制器(优化版)===================== +class PIDController: + """PID速度控制器""" + def __init__(self): + self.kp = 0.3 + self.ki = 0.01 + self.kd = 0.1 + self.prev_error = 0.0 + self.integral = 0.0 + self.integral_limit = 0.5 - # 8. 更新状态 - self.last_steer = steer + def calculate_control(self, target_speed, current_speed): + error = target_speed - current_speed + self.integral = np.clip(self.integral + error * self.ki, -self.integral_limit, self.integral_limit) + derivative = (error - self.prev_error) * self.kd + output = self.kp * error + self.integral + derivative - return steer + throttle = max(0.0, min(1.0, output)) if output > 0 else 0.0 + brake = max(0.0, min(1.0, -output)) if output < 0 else 0.0 - 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 + self.prev_error = error - def calculate(self, target_speed, current_speed): - """PID速度控制""" - error = target_speed - current_speed + from collections import namedtuple + Control = namedtuple('Control', ['throttle', 'brake']) + return Control(throttle=throttle, brake=brake) - 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): +# ===================== Pygame显示(优化版)===================== +class PygameDisplay: + """Pygame摄像头显示类""" + def __init__(self, world, vehicle): 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)) + self.screen = None + self.surface = None + + pygame.init() + self.screen = pygame.display.set_mode((800, 600)) + pygame.display.set_caption("CARLA Camera View (ESC to quit)") + + self._setup_camera() + + def _setup_camera(self): + try: + bp_lib = self.world.get_blueprint_library() + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '800') + camera_bp.set_attribute('image_size_y', '600') + camera_bp.set_attribute('fov', '90') + camera_transform = carla.Transform(carla.Location(x=2.5, z=1.8)) + self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) + self.camera.listen(lambda image: self._process_image(image)) + except Exception as e: + logging.warning(f"摄像头初始化失败:{e}") + self.camera = None + + def _process_image(self, image): + try: + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((image.height, image.width, 4))[:, :, :3] + array = array[:, :, ::-1].swapaxes(0, 1) + self.surface = pygame.surfarray.make_surface(array) + except Exception as e: + logging.warning(f"图像处理失败:{e}") + + def parse_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): + return True + return False + + def render(self): + self.screen.fill((0, 0, 0)) + if self.surface is not None: + self.screen.blit(self.surface, (0, 0)) pygame.display.flip() def destroy(self): if self.camera: self.camera.stop() self.camera.destroy() + pygame.quit() -# ===================== 主函数(核心逻辑)===================== -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() +# ===================== 绘图类(优化版)===================== +class Plotter: + """轨迹+速度绘图类""" + def __init__(self, waypoints): + self.waypoints = waypoints + self.is_initialized = False + self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(14, 6)) + self.x_data = [] + self.y_data = [] + self.time_data = [] + self.speed_data = [] + self.target_speed_data = [] + + def init_plot(self): + try: + waypoints_np = np.array(self.waypoints) + self.ax1.plot(waypoints_np[:, 0], waypoints_np[:, 1], 'ro-', label='Waypoints', markersize=4) + self.ax1.set_xlabel('X (m)') + self.ax1.set_ylabel('Y (m)') + self.ax1.set_title('Vehicle Trajectory') + self.ax1.legend() + self.ax1.grid(True) + + self.ax2.set_xlabel('Time (s)') + self.ax2.set_ylabel('Speed (km/h)') + self.ax2.set_title('Speed vs Time') + self.ax2.grid(True) + + plt.ion() + plt.show(block=False) + self.is_initialized = True + except Exception as e: + logging.error(f"绘图初始化失败:{e}") + self.is_initialized = False + + def update_plot(self, time, x, y, speed, target_speed): + if not self.is_initialized: + return + + try: + self.x_data.append(x) + self.y_data.append(y) + self.time_data.append(time) + self.speed_data.append(speed) + self.target_speed_data.append(target_speed) + + self.ax1.plot(self.x_data, self.y_data, 'b-', label='Trajectory' if len(self.x_data) == 1 else "", linewidth=1) + if len(self.x_data) == 1: + self.ax1.legend() + + self.ax2.plot(self.time_data, self.speed_data, 'b-', label='Current Speed' if len(self.time_data) == 1 else "", linewidth=1) + self.ax2.plot(self.time_data, self.target_speed_data, 'r--', label='Target Speed' if len(self.time_data) == 1 else "", linewidth=1) + if len(self.time_data) == 1: + self.ax2.legend() + + self.fig.canvas.draw() + self.fig.canvas.flush_events() + except Exception as e: + logging.warning(f"绘图更新失败:{e}") + + def cleanup_plot(self): + if self.is_initialized: + plt.ioff() + plt.close(self.fig) + self.is_initialized = False + +# ===================== 配置类(动态航点版)===================== +class Config: + """仿真配置类""" + # 动态航点配置 + NUM_WAYPOINTS = 50 # 生成的航点数量 + WAYPOINT_SPACING = 2.0 # 航点间距(米) + TARGET_SPEED = 30.0 # 统一目标速度(km/h) + + # 控制器参数 + WAYPOINT_THRESHOLD = 1.5 + MIN_SPEED_STANLEY = 1e-4 + STANLEY_K = 0.4 + MAX_STEER_DEG = 40.0 + MAX_STEER_RAD = math.radians(MAX_STEER_DEG) + + # CARLA配置 + CARLA_HOST = "localhost" + CARLA_PORT = 2000 + CARLA_TIMEOUT = 10.0 + + # 车辆配置(Town03主干道生成位置) + VEHICLE_MODEL = "vehicle.tesla.model3" + SPAWN_LOCATION = carla.Location(x=100.0, y=10.0, z=0.5) # Town03开阔主干道 + SPAWN_YAW = 0.0 # 初始朝向 + + # 观察者视角(适配新生成位置) + SPECTATOR_LOCATION = carla.Location(x=110.0, y=10.0, z=8.0) + SPECTATOR_ROTATION = carla.Rotation(pitch=-30.0, yaw=0.0, roll=0.0) + +# ===================== 工具类(优化版)===================== +class Utils: + """工具函数类""" + + @staticmethod + def normalize_angle(angle: float) -> float: + while angle > np.pi: + angle -= 2 * np.pi + while angle < -np.pi: + angle += 2 * np.pi + return angle + + @staticmethod + def get_vehicle_speed(vehicle: carla.Vehicle, unit: str = "kmh") -> float: + vel = vehicle.get_velocity() + speed_mps = np.linalg.norm([vel.x, vel.y, vel.z]) + return speed_mps * 3.6 if unit == "kmh" else speed_mps + + @staticmethod + def calculate_cte(vehicle_loc: carla.Location, prev_wp: List[float], target_wp: List[float]) -> float: + x1, y1 = prev_wp[0], prev_wp[1] + x2, y2 = target_wp[0], target_wp[1] + x, y = vehicle_loc.x, vehicle_loc.y + + dx = x2 - x1 + dy = y2 - y1 + if abs(dx) < 1e-6 and abs(dy) < 1e-6: + return math.hypot(x - x1, y - y1) + + if abs(dx) < 1e-6: + cte = x - x1 + else: + slope = dy / dx + a = -slope + b = 1.0 + c = slope * x1 - y1 + cte = (a * x + b * y + c) / np.sqrt(a ** 2 + b ** 2) + + yaw_path = np.arctan2(dy, dx) + yaw_ct = np.arctan2(y - y1, x - x1) + yaw_diff = Utils.normalize_angle(yaw_path - yaw_ct) + + return abs(cte) if yaw_diff > 0 else -abs(cte) + + @staticmethod + def calculate_distance_2d(loc1: carla.Location, loc2: carla.Location) -> float: + dx = loc1.x - loc2.x + dy = loc1.y - loc2.y + return math.sqrt(dx ** 2 + dy ** 2) + + @staticmethod + def generate_road_waypoints(world, start_loc: carla.Location, num_waypoints: int, spacing: float) -> List[List[float]]: + """ + 沿道路动态生成航点(核心:避免障碍物) + :param world: CARLA世界对象 + :param start_loc: 起始位置 + :param num_waypoints: 航点数量 + :param spacing: 航点间距 + :return: 航点列表 [[x, y, speed], ...] + """ map = world.get_map() + waypoints = [] + current_waypoint = map.get_waypoint(start_loc) + + for i in range(num_waypoints): + # 添加当前航点(x, y, 目标速度) + waypoints.append([current_waypoint.transform.location.x, current_waypoint.transform.location.y, Config.TARGET_SPEED]) + # 获取下一个道路航点(沿道路前进) + next_waypoints = current_waypoint.next(spacing) + if not next_waypoints: + break + current_waypoint = next_waypoints[0] + + logging.info(f"动态生成了 {len(waypoints)} 个道路航点") + return waypoints + +# ===================== 航点管理器(优化版)===================== +class WaypointManager: + """航点管理类""" + def __init__(self, waypoints: List[List[float]], threshold: float): + self.waypoints = waypoints + self.threshold = threshold + self.current_target_id = 1 + + def update_target(self, vehicle_loc: carla.Location) -> None: + if self.current_target_id >= len(self.waypoints) - 1: + return + + target_wp = self.waypoints[self.current_target_id] + target_loc = carla.Location(x=target_wp[0], y=target_wp[1]) + distance = Utils.calculate_distance_2d(vehicle_loc, target_loc) + + if distance < self.threshold: + self._log_waypoint_switch(distance) + self.current_target_id += 1 + + def _log_waypoint_switch(self, distance: float) -> None: + reached_id = self.current_target_id + reached_coords = (self.waypoints[reached_id][0], self.waypoints[reached_id][1]) + log_msg = f"到达航点 {reached_id} (X={reached_coords[0]:.1f}, Y={reached_coords[1]:.1f}, 距离={distance:.1f}m)" + + if self.current_target_id + 1 < len(self.waypoints): + next_id = self.current_target_id + 1 + next_coords = (self.waypoints[next_id][0], self.waypoints[next_id][1]) + log_msg += f",新目标:航点 {next_id} (X={next_coords[0]:.1f}, Y={next_coords[1]:.1f})" + + logging.info(log_msg) + + def get_current_target(self) -> List[float]: + return self.waypoints[self.current_target_id] + + def get_target_speed(self) -> float: + return self.get_current_target()[2] if self.current_target_id < len(self.waypoints) else 0.0 + +# ===================== Stanley控制器(优化版)===================== +class StanleyController: + """Stanley横向控制器""" + def __init__(self, k: float, max_steer_rad: float, min_speed: float): + self.k = k + self.max_steer_rad = max_steer_rad + self.min_speed = min_speed + self.prev_steer = 0.0 + self.smoothing_factor = 0.7 + + def calculate_steer(self, vehicle: carla.Vehicle, waypoints: List[List[float]], target_id: int) -> Tuple[float, float]: + if target_id < 1: + return 0.0, 0.0 + + vehicle_transform = vehicle.get_transform() + vehicle_loc = vehicle_transform.location + vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) + vehicle_speed = Utils.get_vehicle_speed(vehicle, unit="mps") + + prev_wp = waypoints[target_id - 1] + target_wp = waypoints[target_id] + + yaw_path = np.arctan2(target_wp[1] - prev_wp[1], target_wp[0] - prev_wp[0]) + yaw_error = Utils.normalize_angle(yaw_path - vehicle_yaw) + cte = Utils.calculate_cte(vehicle_loc, prev_wp, target_wp) + + # 动态K值 + if vehicle_speed < 5: + k = self.k * 2 + elif vehicle_speed > 15: + k = self.k * 0.5 + else: + k = self.k + + safe_speed = max(vehicle_speed, self.min_speed) + cte_steer = np.arctan(k * cte / safe_speed) + + total_steer = Utils.normalize_angle(yaw_error + cte_steer) + total_steer = np.clip(total_steer, -self.max_steer_rad, self.max_steer_rad) + steer_carla = total_steer / self.max_steer_rad + + # 平滑转向 + steer_carla = self.smoothing_factor * self.prev_steer + (1 - self.smoothing_factor) * steer_carla + self.prev_steer = steer_carla + + return steer_carla, cte + +# ===================== 主仿真类(动态航点版)===================== +class CarlaSimulation: + """CARLA仿真主类""" + def __init__(self): + logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s") + self.logger = logging.getLogger(__name__) + + self.config = Config() + self.client: Optional[carla.Client] = None + self.world: Optional[carla.World] = None + self.vehicle: Optional[carla.Vehicle] = None + + # 动态航点(后续生成) + self.waypoints = [] + self.waypoint_manager = None + self.stanley_controller = StanleyController( + k=self.config.STANLEY_K, + max_steer_rad=self.config.MAX_STEER_RAD, + min_speed=self.config.MIN_SPEED_STANLEY + ) + self.pid_controller = PIDController() + + self.plotter: Optional[Plotter] = None + self.pygame_display: Optional[PygameDisplay] = None + self.sim_start_time: float = 0.0 + + def connect_carla(self) -> bool: + try: + self.client = carla.Client(self.config.CARLA_HOST, self.config.CARLA_PORT) + self.client.set_timeout(self.config.CARLA_TIMEOUT) + self.world = self.client.get_world() + self.logger.info(f"成功连接到CARLA世界:{self.world.get_map().name}") + return True + except Exception as e: + self.logger.error(f"连接CARLA失败:{e}") + return False + + def cleanup_vehicles(self) -> None: + self.logger.info("清理历史车辆...") + actors = self.world.get_actors().filter("vehicle.*") + removed_count = 0 + for actor in actors: + if actor.attributes.get("role_name") == "my_car": + if actor.destroy(): + removed_count += 1 + self.logger.info(f"销毁车辆:{actor.type_id} (ID: {actor.id})") + self.logger.info(f"共销毁 {removed_count} 辆历史车辆") + + def spawn_vehicle(self) -> bool: + """在开阔道路生成车辆(避开障碍物)""" + # 获取生成位置的道路Waypoint + map = self.world.get_map() + spawn_waypoint = map.get_waypoint(self.config.SPAWN_LOCATION) + if not spawn_waypoint: + self.logger.error("生成位置不在道路上") + return False + + # 生成Transform + spawn_transform = spawn_waypoint.transform + spawn_transform.location.z += 0.5 + spawn_transform.rotation.yaw = self.config.SPAWN_YAW + + # 加载车辆蓝图 + bp_lib = self.world.get_blueprint_library() + vehicle_bp = bp_lib.filter(self.config.VEHICLE_MODEL)[0] + vehicle_bp.set_attribute("role_name", "my_car") + + # 生成车辆(重试) + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) + if self.vehicle is None: + spawn_transform.location.z += 0.5 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) + if self.vehicle is None: + self.logger.error(f"无法生成车辆:{spawn_transform.location}") + return False + + # 动态生成道路航点(核心:避开障碍物) + self.waypoints = Utils.generate_road_waypoints( + world=self.world, + start_loc=self.config.SPAWN_LOCATION, + num_waypoints=self.config.NUM_WAYPOINTS, + spacing=self.config.WAYPOINT_SPACING + ) + if len(self.waypoints) < 2: + self.logger.error("生成的航点数量不足") + return False + + # 初始化航点管理器 + self.waypoint_manager = WaypointManager( + waypoints=self.waypoints, + threshold=self.config.WAYPOINT_THRESHOLD + ) - # 清理现有车辆 - 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 + self.logger.info(f"成功生成车辆:{self.vehicle.type_id} (ID: {self.vehicle.id})") + self.logger.info(f"生成位置:{spawn_transform.location}, 初始朝向:{spawn_transform.rotation.yaw:.1f}度") + return True - # 7. 应用控制 + def setup_spectator(self) -> None: + """设置观察者视角(清晰查看车辆前方)""" + spectator = self.world.get_spectator() + spectator_transform = carla.Transform( + self.config.SPECTATOR_LOCATION, + self.config.SPECTATOR_ROTATION + ) + spectator.set_transform(spectator_transform) + self.logger.info("已设置观察者视角") + + def init_visualization(self) -> None: + self.plotter = Plotter(waypoints=self.waypoints) + self.plotter.init_plot() + self.logger.info("Plotter初始化完成") + + self.pygame_display = PygameDisplay(self.world, self.vehicle) + self.logger.info("Pygame显示初始化完成") + + def run_simulation(self) -> None: + self.sim_start_time = time.time() + self.logger.info("开始仿真循环...") + + while True: + if self.pygame_display.parse_events(): + self.logger.info("用户请求退出仿真") + break + + self.world.wait_for_tick() + + # 获取车辆状态 + vehicle_transform = self.vehicle.get_transform() + vehicle_loc = vehicle_transform.location + current_speed = Utils.get_vehicle_speed(vehicle=self.vehicle, unit="kmh") + sim_time = time.time() - self.sim_start_time + + # 更新航点 + self.waypoint_manager.update_target(vehicle_loc) + target_speed = self.waypoint_manager.get_target_speed() + + # 计算控制 + throttle, brake = self.pid_controller.calculate_control(target_speed, current_speed) + steer, cte = self.stanley_controller.calculate_steer( + vehicle=self.vehicle, + waypoints=self.waypoints, + target_id=self.waypoint_manager.current_target_id + ) + + # 应用控制 control = carla.VehicleControl() - control.steer = steer control.throttle = throttle control.brake = brake + control.steer = steer 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("仿真结束") + control.manual_gear_shift = False + self.vehicle.apply_control(control) + + # 调试日志 + if int(sim_time) % 1 == 0 and not hasattr(self, f"_logged_{int(sim_time)}"): + setattr(self, f"_logged_{int(sim_time)}", True) + self.logger.info( + f"时间:{sim_time:.1f}s | " + f"速度:{current_speed:.1f}km/h | " + f"目标速度:{target_speed:.1f}km/h | " + f"转向角:{steer:.2f} | " + f"横向偏差:{cte:.2f}m" + ) + + # 更新可视化 + if self.plotter and self.plotter.is_initialized: + try: + self.plotter.update_plot(sim_time, vehicle_loc.x, vehicle_loc.y, current_speed, target_speed) + except Exception as e: + self.logger.warning(f"绘图更新失败:{e}") + self.plotter.cleanup_plot() + self.plotter = None + + if self.pygame_display: + self.pygame_display.render() + + def cleanup(self) -> None: + self.logger.info("开始清理资源...") + + if self.pygame_display: + self.pygame_display.destroy() + self.logger.info("Pygame显示已销毁") + + if self.plotter and self.plotter.is_initialized: + self.plotter.cleanup_plot() + self.logger.info("绘图器已清理") + + if self.vehicle and self.vehicle.is_alive: + self.vehicle.destroy() + self.logger.info("车辆已销毁") + + self.logger.info("资源清理完成") + + def start(self) -> None: + try: + if not self.connect_carla(): + return + + self.cleanup_vehicles() + + if not self.spawn_vehicle(): + return + + self.setup_spectator() + self.init_visualization() + self.run_simulation() + + except KeyboardInterrupt: + self.logger.info("用户中断仿真") + except Exception as e: + self.logger.error(f"仿真异常:{e}", exc_info=True) + finally: + self.cleanup() + +# ===================== 主函数 ===================== +def main(): + simulation = CarlaSimulation() + simulation.start() if __name__ == "__main__": main() \ No newline at end of file From 3edfc11fe39c511973bf6aa1eea00e13981c7456 Mon Sep 17 00:00:00 2001 From: Liyang2302 <2358507952@qq.com> Date: Sat, 20 Dec 2025 12:29:37 +0800 Subject: [PATCH 11/11] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E8=BD=A6=E8=BE=86?= =?UTF-8?q?=E8=87=AA=E4=B8=BB=E9=81=93=E8=B7=AF=E8=88=AA=E7=82=B9=E8=B7=9F?= =?UTF-8?q?=E9=9A=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomous_driving_car/main.py | 826 ++++++++++------------------- 1 file changed, 290 insertions(+), 536 deletions(-) diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py index 64f88ed002..2e6e2fd2f2 100644 --- a/src/autonomous_driving_car/main.py +++ b/src/autonomous_driving_car/main.py @@ -1,214 +1,48 @@ #!/usr/bin/env python +# -*- coding: utf-8 -*- """ -CARLA Waypoint Following - Dynamic Road Waypoints (sd_6/__main__.py) - -核心改进: -1. 动态生成沿道路的航点,替代固定坐标,避开障碍物 -2. 选择地图开阔区域生成车辆(Town03主干道) -3. 增加障碍物检测,确保行驶路段安全 -4. 优化航点跟踪逻辑,适配动态道路 +CARLA Waypoint Following(原生道路航点版) +核心:直接使用CARLA地图的道路航点,车辆100%沿道路行驶 """ -# ===================== 系统导入 & 路径配置 ===================== import sys import os import carla import numpy as np -import time import math -import logging -from typing import List, Tuple, Optional, Union -import pygame import matplotlib.pyplot as plt +import cv2 +import time -# 获取当前脚本目录并加入搜索路径 -current_dir = os.path.dirname(os.path.abspath(__file__)) -sys.path.append(current_dir) - -# ===================== PID控制器(优化版)===================== -class PIDController: - """PID速度控制器""" - def __init__(self): - self.kp = 0.3 - self.ki = 0.01 - self.kd = 0.1 - self.prev_error = 0.0 - self.integral = 0.0 - self.integral_limit = 0.5 - - def calculate_control(self, target_speed, current_speed): - error = target_speed - current_speed - self.integral = np.clip(self.integral + error * self.ki, -self.integral_limit, self.integral_limit) - derivative = (error - self.prev_error) * self.kd - output = self.kp * error + self.integral + derivative - - throttle = max(0.0, min(1.0, output)) if output > 0 else 0.0 - brake = max(0.0, min(1.0, -output)) if output < 0 else 0.0 - - self.prev_error = error - - from collections import namedtuple - Control = namedtuple('Control', ['throttle', 'brake']) - return Control(throttle=throttle, brake=brake) - -# ===================== Pygame显示(优化版)===================== -class PygameDisplay: - """Pygame摄像头显示类""" - def __init__(self, world, vehicle): - self.world = world - self.vehicle = vehicle - self.camera = None - self.screen = None - self.surface = None - - pygame.init() - self.screen = pygame.display.set_mode((800, 600)) - pygame.display.set_caption("CARLA Camera View (ESC to quit)") - - self._setup_camera() - - def _setup_camera(self): - try: - bp_lib = self.world.get_blueprint_library() - camera_bp = bp_lib.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '800') - camera_bp.set_attribute('image_size_y', '600') - camera_bp.set_attribute('fov', '90') - camera_transform = carla.Transform(carla.Location(x=2.5, z=1.8)) - self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - self.camera.listen(lambda image: self._process_image(image)) - except Exception as e: - logging.warning(f"摄像头初始化失败:{e}") - self.camera = None - - def _process_image(self, image): - try: - array = np.frombuffer(image.raw_data, dtype=np.uint8) - array = array.reshape((image.height, image.width, 4))[:, :, :3] - array = array[:, :, ::-1].swapaxes(0, 1) - self.surface = pygame.surfarray.make_surface(array) - except Exception as e: - logging.warning(f"图像处理失败:{e}") - - def parse_events(self): - for event in pygame.event.get(): - if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): - return True - return False - - def render(self): - self.screen.fill((0, 0, 0)) - if self.surface is not None: - self.screen.blit(self.surface, (0, 0)) - pygame.display.flip() - - def destroy(self): - if self.camera: - self.camera.stop() - self.camera.destroy() - pygame.quit() - -# ===================== 绘图类(优化版)===================== -class Plotter: - """轨迹+速度绘图类""" - def __init__(self, waypoints): - self.waypoints = waypoints - self.is_initialized = False - self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(14, 6)) - self.x_data = [] - self.y_data = [] - self.time_data = [] - self.speed_data = [] - self.target_speed_data = [] - - def init_plot(self): - try: - waypoints_np = np.array(self.waypoints) - self.ax1.plot(waypoints_np[:, 0], waypoints_np[:, 1], 'ro-', label='Waypoints', markersize=4) - self.ax1.set_xlabel('X (m)') - self.ax1.set_ylabel('Y (m)') - self.ax1.set_title('Vehicle Trajectory') - self.ax1.legend() - self.ax1.grid(True) - - self.ax2.set_xlabel('Time (s)') - self.ax2.set_ylabel('Speed (km/h)') - self.ax2.set_title('Speed vs Time') - self.ax2.grid(True) - - plt.ion() - plt.show(block=False) - self.is_initialized = True - except Exception as e: - logging.error(f"绘图初始化失败:{e}") - self.is_initialized = False - - def update_plot(self, time, x, y, speed, target_speed): - if not self.is_initialized: - return - - try: - self.x_data.append(x) - self.y_data.append(y) - self.time_data.append(time) - self.speed_data.append(speed) - self.target_speed_data.append(target_speed) - - self.ax1.plot(self.x_data, self.y_data, 'b-', label='Trajectory' if len(self.x_data) == 1 else "", linewidth=1) - if len(self.x_data) == 1: - self.ax1.legend() - - self.ax2.plot(self.time_data, self.speed_data, 'b-', label='Current Speed' if len(self.time_data) == 1 else "", linewidth=1) - self.ax2.plot(self.time_data, self.target_speed_data, 'r--', label='Target Speed' if len(self.time_data) == 1 else "", linewidth=1) - if len(self.time_data) == 1: - self.ax2.legend() - - self.fig.canvas.draw() - self.fig.canvas.flush_events() - except Exception as e: - logging.warning(f"绘图更新失败:{e}") - - def cleanup_plot(self): - if self.is_initialized: - plt.ioff() - plt.close(self.fig) - self.is_initialized = False - -# ===================== 配置类(动态航点版)===================== +# ===================== 核心配置 ===================== class Config: - """仿真配置类""" - # 动态航点配置 - NUM_WAYPOINTS = 50 # 生成的航点数量 - WAYPOINT_SPACING = 2.0 # 航点间距(米) - TARGET_SPEED = 30.0 # 统一目标速度(km/h) - - # 控制器参数 - WAYPOINT_THRESHOLD = 1.5 - MIN_SPEED_STANLEY = 1e-4 - STANLEY_K = 0.4 - MAX_STEER_DEG = 40.0 - MAX_STEER_RAD = math.radians(MAX_STEER_DEG) - - # CARLA配置 - CARLA_HOST = "localhost" - CARLA_PORT = 2000 - CARLA_TIMEOUT = 10.0 - - # 车辆配置(Town03主干道生成位置) + # 速度控制 + TARGET_SPEED = 20.0 # km/h + PID_KP = 0.3 + PID_KI = 0.02 + PID_KD = 0.01 + + # 纯追踪算法参数 + LOOKAHEAD_DISTANCE = 8.0 # 前瞻距离(米) + MAX_STEER_ANGLE = 30.0 # 最大转向角(度) + + # 摄像头配置 + CAMERA_WIDTH = 800 + CAMERA_HEIGHT = 600 + CAMERA_FOV = 90 + + # 车辆配置 VEHICLE_MODEL = "vehicle.tesla.model3" - SPAWN_LOCATION = carla.Location(x=100.0, y=10.0, z=0.5) # Town03开阔主干道 - SPAWN_YAW = 0.0 # 初始朝向 - - # 观察者视角(适配新生成位置) - SPECTATOR_LOCATION = carla.Location(x=110.0, y=10.0, z=8.0) - SPECTATOR_ROTATION = carla.Rotation(pitch=-30.0, yaw=0.0, roll=0.0) -# ===================== 工具类(优化版)===================== -class Utils: - """工具函数类""" + # 可视化配置 + PLOT_SIZE = (12, 10) + WAYPOINT_COUNT = 50 # 预先生成的道路航点数量 +# ===================== 工具类 ===================== +class Tools: @staticmethod - def normalize_angle(angle: float) -> float: + def normalize_angle(angle): + """将角度归一化到[-pi, pi]""" while angle > np.pi: angle -= 2 * np.pi while angle < -np.pi: @@ -216,372 +50,292 @@ def normalize_angle(angle: float) -> float: return angle @staticmethod - def get_vehicle_speed(vehicle: carla.Vehicle, unit: str = "kmh") -> float: + def get_vehicle_pose(vehicle): + """获取车辆的位置、朝向和速度""" + transform = vehicle.get_transform() + loc = transform.location + yaw = math.radians(transform.rotation.yaw) vel = vehicle.get_velocity() - speed_mps = np.linalg.norm([vel.x, vel.y, vel.z]) - return speed_mps * 3.6 if unit == "kmh" else speed_mps + speed = 3.6 * np.linalg.norm([vel.x, vel.y, vel.z]) + return loc, yaw, speed @staticmethod - def calculate_cte(vehicle_loc: carla.Location, prev_wp: List[float], target_wp: List[float]) -> float: - x1, y1 = prev_wp[0], prev_wp[1] - x2, y2 = target_wp[0], target_wp[1] - x, y = vehicle_loc.x, vehicle_loc.y - - dx = x2 - x1 - dy = y2 - y1 - if abs(dx) < 1e-6 and abs(dy) < 1e-6: - return math.hypot(x - x1, y - y1) - - if abs(dx) < 1e-6: - cte = x - x1 - else: - slope = dy / dx - a = -slope - b = 1.0 - c = slope * x1 - y1 - cte = (a * x + b * y + c) / np.sqrt(a ** 2 + b ** 2) - - yaw_path = np.arctan2(dy, dx) - yaw_ct = np.arctan2(y - y1, x - x1) - yaw_diff = Utils.normalize_angle(yaw_path - yaw_ct) - - return abs(cte) if yaw_diff > 0 else -abs(cte) + def clear_all_actors(world): + """清理所有车辆和传感器""" + for actor in world.get_actors(): + try: + if actor.type_id.startswith('vehicle') or actor.type_id.startswith('sensor'): + actor.destroy() + except: + pass + time.sleep(0.5) + print("已清理所有残留Actor") @staticmethod - def calculate_distance_2d(loc1: carla.Location, loc2: carla.Location) -> float: - dx = loc1.x - loc2.x - dy = loc1.y - loc2.y - return math.sqrt(dx ** 2 + dy ** 2) + def focus_vehicle(world, vehicle): + """将CARLA客户端视角聚焦到车辆""" + spectator = world.get_spectator() + t = vehicle.get_transform() + spectator.set_transform(carla.Transform(t.location + carla.Location(x=0, y=-8, z=5), t.rotation)) + print("CARLA客户端已聚焦到车辆") @staticmethod - def generate_road_waypoints(world, start_loc: carla.Location, num_waypoints: int, spacing: float) -> List[List[float]]: + def generate_road_waypoints(world, start_loc, count=50, step=2.0): """ - 沿道路动态生成航点(核心:避免障碍物) + 从起点沿道路生成连续的原生航点 :param world: CARLA世界对象 - :param start_loc: 起始位置 - :param num_waypoints: 航点数量 - :param spacing: 航点间距 - :return: 航点列表 [[x, y, speed], ...] + :param start_loc: 起点位置 + :param count: 航点数量 + :param step: 每个航点的步长(米) + :return: 航点列表[(x, y, z), ...] """ - map = world.get_map() waypoints = [] - current_waypoint = map.get_waypoint(start_loc) - - for i in range(num_waypoints): - # 添加当前航点(x, y, 目标速度) - waypoints.append([current_waypoint.transform.location.x, current_waypoint.transform.location.y, Config.TARGET_SPEED]) - # 获取下一个道路航点(沿道路前进) - next_waypoints = current_waypoint.next(spacing) - if not next_waypoints: - break - current_waypoint = next_waypoints[0] - - logging.info(f"动态生成了 {len(waypoints)} 个道路航点") + map = world.get_map() + wp = map.get_waypoint(start_loc) + for i in range(count): + waypoints.append((wp.transform.location.x, wp.transform.location.y, wp.transform.location.z)) + # 沿道路下一个航点(直走,不考虑分叉) + wp = wp.next(step)[0] + print(f"生成了{len(waypoints)}个原生道路航点") return waypoints -# ===================== 航点管理器(优化版)===================== -class WaypointManager: - """航点管理类""" - def __init__(self, waypoints: List[List[float]], threshold: float): - self.waypoints = waypoints - self.threshold = threshold - self.current_target_id = 1 - - def update_target(self, vehicle_loc: carla.Location) -> None: - if self.current_target_id >= len(self.waypoints) - 1: - return - - target_wp = self.waypoints[self.current_target_id] - target_loc = carla.Location(x=target_wp[0], y=target_wp[1]) - distance = Utils.calculate_distance_2d(vehicle_loc, target_loc) - - if distance < self.threshold: - self._log_waypoint_switch(distance) - self.current_target_id += 1 - - def _log_waypoint_switch(self, distance: float) -> None: - reached_id = self.current_target_id - reached_coords = (self.waypoints[reached_id][0], self.waypoints[reached_id][1]) - log_msg = f"到达航点 {reached_id} (X={reached_coords[0]:.1f}, Y={reached_coords[1]:.1f}, 距离={distance:.1f}m)" - - if self.current_target_id + 1 < len(self.waypoints): - next_id = self.current_target_id + 1 - next_coords = (self.waypoints[next_id][0], self.waypoints[next_id][1]) - log_msg += f",新目标:航点 {next_id} (X={next_coords[0]:.1f}, Y={next_coords[1]:.1f})" - - logging.info(log_msg) - - def get_current_target(self) -> List[float]: - return self.waypoints[self.current_target_id] - - def get_target_speed(self) -> float: - return self.get_current_target()[2] if self.current_target_id < len(self.waypoints) else 0.0 - -# ===================== Stanley控制器(优化版)===================== -class StanleyController: - """Stanley横向控制器""" - def __init__(self, k: float, max_steer_rad: float, min_speed: float): - self.k = k - self.max_steer_rad = max_steer_rad - self.min_speed = min_speed - self.prev_steer = 0.0 - self.smoothing_factor = 0.7 - - def calculate_steer(self, vehicle: carla.Vehicle, waypoints: List[List[float]], target_id: int) -> Tuple[float, float]: - if target_id < 1: - return 0.0, 0.0 - - vehicle_transform = vehicle.get_transform() - vehicle_loc = vehicle_transform.location - vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) - vehicle_speed = Utils.get_vehicle_speed(vehicle, unit="mps") - - prev_wp = waypoints[target_id - 1] - target_wp = waypoints[target_id] - - yaw_path = np.arctan2(target_wp[1] - prev_wp[1], target_wp[0] - prev_wp[0]) - yaw_error = Utils.normalize_angle(yaw_path - vehicle_yaw) - cte = Utils.calculate_cte(vehicle_loc, prev_wp, target_wp) - - # 动态K值 - if vehicle_speed < 5: - k = self.k * 2 - elif vehicle_speed > 15: - k = self.k * 0.5 - else: - k = self.k - - safe_speed = max(vehicle_speed, self.min_speed) - cte_steer = np.arctan(k * cte / safe_speed) - - total_steer = Utils.normalize_angle(yaw_error + cte_steer) - total_steer = np.clip(total_steer, -self.max_steer_rad, self.max_steer_rad) - steer_carla = total_steer / self.max_steer_rad - - # 平滑转向 - steer_carla = self.smoothing_factor * self.prev_steer + (1 - self.smoothing_factor) * steer_carla - self.prev_steer = steer_carla - - return steer_carla, cte - -# ===================== 主仿真类(动态航点版)===================== -class CarlaSimulation: - """CARLA仿真主类""" - def __init__(self): - logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s") - self.logger = logging.getLogger(__name__) - - self.config = Config() - self.client: Optional[carla.Client] = None - self.world: Optional[carla.World] = None - self.vehicle: Optional[carla.Vehicle] = None - - # 动态航点(后续生成) - self.waypoints = [] - self.waypoint_manager = None - self.stanley_controller = StanleyController( - k=self.config.STANLEY_K, - max_steer_rad=self.config.MAX_STEER_RAD, - min_speed=self.config.MIN_SPEED_STANLEY - ) - self.pid_controller = PIDController() - - self.plotter: Optional[Plotter] = None - self.pygame_display: Optional[PygameDisplay] = None - self.sim_start_time: float = 0.0 - - def connect_carla(self) -> bool: - try: - self.client = carla.Client(self.config.CARLA_HOST, self.config.CARLA_PORT) - self.client.set_timeout(self.config.CARLA_TIMEOUT) - self.world = self.client.get_world() - self.logger.info(f"成功连接到CARLA世界:{self.world.get_map().name}") - return True - except Exception as e: - self.logger.error(f"连接CARLA失败:{e}") - return False - - def cleanup_vehicles(self) -> None: - self.logger.info("清理历史车辆...") - actors = self.world.get_actors().filter("vehicle.*") - removed_count = 0 - for actor in actors: - if actor.attributes.get("role_name") == "my_car": - if actor.destroy(): - removed_count += 1 - self.logger.info(f"销毁车辆:{actor.type_id} (ID: {actor.id})") - self.logger.info(f"共销毁 {removed_count} 辆历史车辆") - - def spawn_vehicle(self) -> bool: - """在开阔道路生成车辆(避开障碍物)""" - # 获取生成位置的道路Waypoint - map = self.world.get_map() - spawn_waypoint = map.get_waypoint(self.config.SPAWN_LOCATION) - if not spawn_waypoint: - self.logger.error("生成位置不在道路上") - return False - - # 生成Transform - spawn_transform = spawn_waypoint.transform - spawn_transform.location.z += 0.5 - spawn_transform.rotation.yaw = self.config.SPAWN_YAW - - # 加载车辆蓝图 - bp_lib = self.world.get_blueprint_library() - vehicle_bp = bp_lib.filter(self.config.VEHICLE_MODEL)[0] - vehicle_bp.set_attribute("role_name", "my_car") - - # 生成车辆(重试) - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) - if self.vehicle is None: - spawn_transform.location.z += 0.5 - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) - if self.vehicle is None: - self.logger.error(f"无法生成车辆:{spawn_transform.location}") - return False - - # 动态生成道路航点(核心:避开障碍物) - self.waypoints = Utils.generate_road_waypoints( - world=self.world, - start_loc=self.config.SPAWN_LOCATION, - num_waypoints=self.config.NUM_WAYPOINTS, - spacing=self.config.WAYPOINT_SPACING - ) - if len(self.waypoints) < 2: - self.logger.error("生成的航点数量不足") - return False - - # 初始化航点管理器 - self.waypoint_manager = WaypointManager( - waypoints=self.waypoints, - threshold=self.config.WAYPOINT_THRESHOLD - ) - - self.logger.info(f"成功生成车辆:{self.vehicle.type_id} (ID: {self.vehicle.id})") - self.logger.info(f"生成位置:{spawn_transform.location}, 初始朝向:{spawn_transform.rotation.yaw:.1f}度") - return True - - def setup_spectator(self) -> None: - """设置观察者视角(清晰查看车辆前方)""" - spectator = self.world.get_spectator() - spectator_transform = carla.Transform( - self.config.SPECTATOR_LOCATION, - self.config.SPECTATOR_ROTATION - ) - spectator.set_transform(spectator_transform) - self.logger.info("已设置观察者视角") - - def init_visualization(self) -> None: - self.plotter = Plotter(waypoints=self.waypoints) - self.plotter.init_plot() - self.logger.info("Plotter初始化完成") - - self.pygame_display = PygameDisplay(self.world, self.vehicle) - self.logger.info("Pygame显示初始化完成") - - def run_simulation(self) -> None: - self.sim_start_time = time.time() - self.logger.info("开始仿真循环...") - - while True: - if self.pygame_display.parse_events(): - self.logger.info("用户请求退出仿真") - break - - self.world.wait_for_tick() - - # 获取车辆状态 - vehicle_transform = self.vehicle.get_transform() - vehicle_loc = vehicle_transform.location - current_speed = Utils.get_vehicle_speed(vehicle=self.vehicle, unit="kmh") - sim_time = time.time() - self.sim_start_time - - # 更新航点 - self.waypoint_manager.update_target(vehicle_loc) - target_speed = self.waypoint_manager.get_target_speed() - - # 计算控制 - throttle, brake = self.pid_controller.calculate_control(target_speed, current_speed) - steer, cte = self.stanley_controller.calculate_steer( - vehicle=self.vehicle, - waypoints=self.waypoints, - target_id=self.waypoint_manager.current_target_id - ) - - # 应用控制 - control = carla.VehicleControl() - control.throttle = throttle - control.brake = brake - control.steer = steer - control.hand_brake = False - control.manual_gear_shift = False - self.vehicle.apply_control(control) - - # 调试日志 - if int(sim_time) % 1 == 0 and not hasattr(self, f"_logged_{int(sim_time)}"): - setattr(self, f"_logged_{int(sim_time)}", True) - self.logger.info( - f"时间:{sim_time:.1f}s | " - f"速度:{current_speed:.1f}km/h | " - f"目标速度:{target_speed:.1f}km/h | " - f"转向角:{steer:.2f} | " - f"横向偏差:{cte:.2f}m" - ) +# ===================== 摄像头回调 ===================== +def camera_callback(image, data_dict): + array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))[:, :, :3] + data_dict['image'] = array + +# ===================== 控制器 ===================== +class PIDSpeedController: + def __init__(self, config): + self.kp = config.PID_KP + self.ki = config.PID_KI + self.kd = config.PID_KD + self.prev_err = 0.0 + self.integral = 0.0 + self.target_speed = config.TARGET_SPEED + + def calculate(self, current_speed): + err = self.target_speed - current_speed + self.integral = np.clip(self.integral + err * 0.05, -1.0, 1.0) + deriv = (err - self.prev_err) / 0.05 + output = self.kp * err + self.ki * self.integral + self.kd * deriv + self.prev_err = err + return np.clip(output, 0.1, 1.0) + +class PurePursuitController: + def __init__(self, config): + self.lookahead_dist = config.LOOKAHEAD_DISTANCE + self.max_steer_rad = math.radians(config.MAX_STEER_ANGLE) + + def calculate_steer(self, vehicle_loc, vehicle_yaw, waypoints): + """ + 纯追踪算法计算转向角 + :param vehicle_loc: 车辆位置 + :param vehicle_yaw: 车辆朝向(弧度) + :param waypoints: 道路航点列表 + :return: 转向角(-1~1) + """ + # 1. 将航点转换为车辆坐标系 + wp_coords = np.array(waypoints) + vehicle_x = vehicle_loc.x + vehicle_y = vehicle_loc.y + + # 旋转和平移(车辆坐标系:x向前,y向左) + cos_yaw = math.cos(vehicle_yaw) + sin_yaw = math.sin(vehicle_yaw) + translated_x = wp_coords[:, 0] - vehicle_x + translated_y = wp_coords[:, 1] - vehicle_y + rotated_x = translated_x * cos_yaw + translated_y * sin_yaw + rotated_y = -translated_x * sin_yaw + translated_y * cos_yaw + + # 2. 找到距离车辆>=前瞻距离的第一个航点 + distances = np.hypot(rotated_x, rotated_y) + valid_wp_indices = np.where(distances >= self.lookahead_dist)[0] + if len(valid_wp_indices) == 0: + return 0.0 + + target_idx = valid_wp_indices[0] + target_x = rotated_x[target_idx] + target_y = rotated_y[target_idx] + + # 3. 计算转向角(纯追踪公式:steer = arctan(2*L*y/(x²+y²)),L为车辆轴距,这里简化为1.0) + L = 1.0 # 车辆轴距(米) + steer_rad = math.atan2(2 * L * target_y, self.lookahead_dist ** 2) + + # 4. 限制转向角 + steer_rad = np.clip(steer_rad, -self.max_steer_rad, self.max_steer_rad) + steer = steer_rad / self.max_steer_rad + + return steer + +# ===================== 可视化 ===================== +class Visualizer: + def __init__(self, config, spawn_loc, initial_waypoints): + self.waypoints = np.array(initial_waypoints) + self.trajectory = [] + self.spawn_loc = (spawn_loc.x, spawn_loc.y) + + plt.rcParams['backend'] = 'TkAgg' + plt.ioff() + self.fig, self.ax = plt.subplots(figsize=config.PLOT_SIZE) + + # 绘制道路航点(CARLA原生) + self.ax.scatter(self.waypoints[:, 0], self.waypoints[:, 1], c='blue', s=50, label='Road Waypoints (CARLA)', zorder=3) + # 绘制生成点 + self.ax.scatter(self.spawn_loc[0], self.spawn_loc[1], c='orange', marker='s', s=150, label='Spawn Point', zorder=5) + # 轨迹和车辆 + self.traj_line, = self.ax.plot([], [], c='red', linewidth=4, label='Vehicle Trajectory', zorder=2) + self.vehicle_dot, = self.ax.plot([], [], c='green', marker='o', markersize=20, label='Vehicle', zorder=6) + + self.ax.set_xlabel('X (m)', fontsize=14) + self.ax.set_ylabel('Y (m)', fontsize=14) + self.ax.set_title('CARLA Road Following (Native Waypoints)', fontsize=16) + self.ax.legend(fontsize=12) + self.ax.grid(True, alpha=0.3) + self.ax.axis('equal') + + plt.show(block=False) + self.fig.canvas.draw() + self.fig.canvas.flush_events() + + def update(self, vehicle_x, vehicle_y, new_waypoints=None): + """更新轨迹和航点""" + self.trajectory.append([vehicle_x, vehicle_y]) + if len(self.trajectory) > 1000: + self.trajectory = self.trajectory[-1000:] + + # 更新轨迹 + traj = np.array(self.trajectory) + self.traj_line.set_data(traj[:, 0], traj[:, 1]) + self.vehicle_dot.set_data(vehicle_x, vehicle_y) + + # 更新航点(如果有新航点) + if new_waypoints is not None: + self.waypoints = np.array(new_waypoints) + self.ax.scatter(self.waypoints[:, 0], self.waypoints[:, 1], c='blue', s=50, zorder=3) + + self.ax.relim() + self.ax.autoscale_view(True, True, True) + self.fig.canvas.draw() + self.fig.canvas.flush_events() - # 更新可视化 - if self.plotter and self.plotter.is_initialized: - try: - self.plotter.update_plot(sim_time, vehicle_loc.x, vehicle_loc.y, current_speed, target_speed) - except Exception as e: - self.logger.warning(f"绘图更新失败:{e}") - self.plotter.cleanup_plot() - self.plotter = None +# ===================== 主函数 ===================== +def main(): + config = Config() + tools = Tools() + + # 初始化OpenCV窗口 + cv2.namedWindow('CARLA Vehicle View', cv2.WINDOW_NORMAL) + cv2.resizeWindow('CARLA Vehicle View', config.CAMERA_WIDTH, config.CAMERA_HEIGHT) + + # 连接CARLA + try: + client = carla.Client('localhost', 2000) + client.set_timeout(30.0) + world = client.load_world('Town03') + map = world.get_map() - if self.pygame_display: - self.pygame_display.render() + # 设置同步模式 + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + # 清理Actor + tools.clear_all_actors(world) + + # 获取道路生成点 + spawn_points = map.get_spawn_points() + spawn_transform = spawn_points[0] + print(f"使用道路生成点:{spawn_transform.location}") + + # 生成车辆 + bp_lib = world.get_blueprint_library() + vehicle_bp = bp_lib.find(config.VEHICLE_MODEL) + vehicle = world.spawn_actor(vehicle_bp, spawn_transform) + if not vehicle: + print("车辆生成失败!") + return + print(f"车辆{config.VEHICLE_MODEL}生成成功") - def cleanup(self) -> None: - self.logger.info("开始清理资源...") + # 聚焦车辆 + tools.focus_vehicle(world, vehicle) - if self.pygame_display: - self.pygame_display.destroy() - self.logger.info("Pygame显示已销毁") + # 挂载摄像头 + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(config.CAMERA_WIDTH)) + camera_bp.set_attribute('image_size_y', str(config.CAMERA_HEIGHT)) + camera_bp.set_attribute('fov', str(config.CAMERA_FOV)) + camera = world.spawn_actor(camera_bp, carla.Transform(carla.Location(x=2.0, z=1.8)), attach_to=vehicle) - if self.plotter and self.plotter.is_initialized: - self.plotter.cleanup_plot() - self.logger.info("绘图器已清理") + # 摄像头数据 + camera_data = {'image': np.zeros((config.CAMERA_HEIGHT, config.CAMERA_WIDTH, 3), dtype=np.uint8)} + camera.listen(lambda img: camera_callback(img, camera_data)) - if self.vehicle and self.vehicle.is_alive: - self.vehicle.destroy() - self.logger.info("车辆已销毁") + # 生成初始道路航点 + initial_waypoints = tools.generate_road_waypoints(world, spawn_transform.location, config.WAYPOINT_COUNT) - self.logger.info("资源清理完成") + # 初始化控制器 + speed_controller = PIDSpeedController(config) + path_controller = PurePursuitController(config) - def start(self) -> None: - try: - if not self.connect_carla(): - return + # 初始化可视化 + visualizer = Visualizer(config, spawn_transform.location, initial_waypoints) - self.cleanup_vehicles() + # 主循环 + while True: + world.tick() - if not self.spawn_vehicle(): - return + # 获取车辆状态 + vehicle_loc, vehicle_yaw, current_speed = tools.get_vehicle_pose(vehicle) - self.setup_spectator() - self.init_visualization() - self.run_simulation() + # 实时更新道路航点(每10帧更新一次,减少计算量) + if world.get_snapshot().frame % 10 == 0: + new_waypoints = tools.generate_road_waypoints(world, vehicle_loc, config.WAYPOINT_COUNT) + else: + new_waypoints = None - except KeyboardInterrupt: - self.logger.info("用户中断仿真") - except Exception as e: - self.logger.error(f"仿真异常:{e}", exc_info=True) - finally: - self.cleanup() + # 更新可视化 + visualizer.update(vehicle_loc.x, vehicle_loc.y, new_waypoints) -# ===================== 主函数 ===================== -def main(): - simulation = CarlaSimulation() - simulation.start() + # 显示摄像头画面 + cv2.imshow('CARLA Vehicle View', camera_data['image']) + if cv2.waitKey(1) & 0xFF == ord('q'): + break -if __name__ == "__main__": + # 计算控制量 + steer = path_controller.calculate_steer(vehicle_loc, vehicle_yaw, initial_waypoints) + throttle = speed_controller.calculate(current_speed) + brake = 1.0 if current_speed > config.TARGET_SPEED * 2 else 0.0 + + # 控制车辆 + vehicle.apply_control(carla.VehicleControl( + throttle=throttle, + brake=brake, + steer=steer, + hand_brake=False, + reverse=False + )) + + # 打印状态 + print(f"速度:{current_speed:.1f}km/h | 位置:({vehicle_loc.x:.1f}, {vehicle_loc.y:.1f}) | 转向:{steer:.2f}", end='\r') + + except Exception as e: + print(f"\n程序异常:{e}") + finally: + # 清理资源 + print("\n清理资源中...") + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + if 'vehicle' in locals(): + vehicle.destroy() + if 'camera' in locals(): + camera.destroy() + cv2.destroyAllWindows() + plt.close('all') + time.sleep(1) + print("仿真结束") + +if __name__ == '__main__': main() \ No newline at end of file