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/3] =?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/3] =?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/3] =?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()