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 1/4] =?UTF-8?q?=E6=8F=90=E4=BA=A4main.py=E6=96=87=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 2/4] =?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 3/4] =?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 4/4] =?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