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/6] =?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/6] =?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/6] =?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/6] =?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 5/6] =?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 6/6] =?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