From e84d4f51cb8958c4b2d41cea1cdb41586e28f573 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Thu, 18 Dec 2025 19:19:44 +0800
Subject: [PATCH 01/26] =?UTF-8?q?=E6=8F=90=E4=BA=A4main.py=E6=96=87?=
=?UTF-8?q?=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 368 +++++++++++++++++++++++++++++
1 file changed, 368 insertions(+)
create mode 100644 src/autonomous_driving_car/main.py
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
new file mode 100644
index 0000000000..10a4076619
--- /dev/null
+++ b/src/autonomous_driving_car/main.py
@@ -0,0 +1,368 @@
+#!/usr/bin/env python
+"""
+CARLA Basic Vehicle Spawn and Spectator Setup (sd_1/__main__.py)
+完全适配 CARLA 0.9.15 版本（无任何天气预设依赖）
+
+This script connects to a CARLA simulator instance, removes any
+pre-existing vehicles with the role 'my_car', spawns a new
+Tesla Model 3 at a default spawn point, and positions the
+spectator camera behind the newly spawned vehicle.
+新增功能：定时循环切换CARLA模拟器的天气（晴天、多云、雨天、雾天、日落等）
+
+The script keeps the simulation running until interrupted (Ctrl+C),
+but the vehicle does not move.
+"""
+
+# 导入CARLA模拟器的Python API
+import carla
+# 导入时间模块，用于延时和计时
+import time
+
+
+def remove_previous_vehicle(world: carla.World) -> None:
+ """
+ 查找并销毁所有角色名为'my_car'的车辆Actor，避免重复生成导致冲突
+
+ Args:
+ world (carla.World): CARLA模拟器的世界对象，用于获取当前所有Actor
+ """
+ print("Searching for previous 'my_car' vehicles...")
+ # 过滤出所有车辆类型的Actor（vehicle.* 匹配所有车辆蓝图）
+ actors = world.get_actors().filter('vehicle.*')
+ # 记录成功销毁的车辆数量
+ removed_count = 0
+
+ for actor in actors:
+ # 检查Actor的角色名是否为'my_car'
+ if actor.attributes.get('role_name') == 'my_car':
+ print(f" - Removing previous vehicle: {actor.type_id} (ID {actor.id})")
+ # 销毁Actor，返回布尔值表示是否成功
+ if actor.destroy():
+ removed_count += 1
+ else:
+ print(f" - Failed to remove vehicle {actor.id}")
+
+ print(f"Removed {removed_count} previous vehicles.")
+
+
+def set_spectator_behind_vehicle(world: carla.World, vehicle: carla.Vehicle) -> None:
+ """
+ 将旁观者相机（spectator）定位到指定车辆的后上方，实现跟随视角
+
+ Args:
+ world (carla.World): CARLA模拟器的世界对象
+ vehicle (carla.Vehicle): 目标车辆Actor，用于获取车辆的位置和姿态
+ """
+ # 获取旁观者相机Actor（CARLA中全局唯一的 spectator）
+ spectator = world.get_spectator()
+ # 获取车辆的当前位姿（位置+旋转）
+ vehicle_transform = vehicle.get_transform()
+ # 获取车辆的前向向量，用于计算相机的相对偏移（保证相机始终在车辆后方）
+ forward_vector = vehicle_transform.get_forward_vector()
+
+ # 计算相机偏移：向后15米，向上6米（基于车辆的前向向量，保证方向正确）
+ camera_offset = carla.Location(
+ x=-15 * forward_vector.x,
+ y=-15 * forward_vector.y,
+ z=6
+ )
+ # 构建旁观者相机的位姿：
+ # 位置 = 车辆位置 + 偏移量
+ # 旋转 = 俯仰角-20°（向下看），偏航角与车辆一致，滚转角为0
+ spectator_transform = carla.Transform(
+ vehicle_transform.location + camera_offset,
+ carla.Rotation(
+ pitch=-20, # 俯仰角，负数表示向下看
+ yaw=vehicle_transform.rotation.yaw, # 偏航角与车辆一致
+ roll=0 # 滚转角，保持水平
+ )
+ )
+
+ # 尝试设置相机位姿，增加异常处理提高鲁棒性
+ try:
+ spectator.set_transform(spectator_transform)
+ print("Spectator camera positioned behind the vehicle.")
+ except Exception as e:
+ print(f"Error setting spectator transform: {e}")
+
+
+def get_weather_presets() -> list:
+ """
+ 纯手动定义天气参数列表（完全不依赖CARLA预设，适配0.9.15）
+ 每个天气通过手动设置WeatherParameters的所有关键参数实现，确保兼容性
+
+ Returns:
+ list: 元组列表，每个元组包含（天气名称，carla.WeatherParameters对象）
+ """
+ # 1. 晴天中午（太阳高悬、无云、无雨、无雾）
+ clear_noon = carla.WeatherParameters(
+ sun_altitude_angle=75.0, # 太阳高度角（75°=中午，天顶为90°）
+ sun_azimuth_angle=90.0, # 太阳方位角
+ cloudiness=0.0, # 云量（0=无云）
+ precipitation=0.0, # 降水量（0=无雨）
+ precipitation_deposits=0.0, # 降水沉积（路面雨水）
+ wind_intensity=5.0, # 风力
+ fog_density=0.0, # 雾密度（0=无雾）
+ fog_distance=0.0, # 雾的可见距离
+ fog_falloff=1.0, # 雾的衰减率
+ wetness=0.0, # 路面湿度
+ scattering_intensity=0.0, # 光的散射强度
+ mie_scattering_scale=0.0, # 米氏散射比例
+ rayleigh_scattering_scale=0.0 # 瑞利散射比例
+ )
+
+ # 2. 多云中午（高云量，阳光散射）
+ cloudy_noon = carla.WeatherParameters(
+ sun_altitude_angle=75.0,
+ sun_azimuth_angle=90.0,
+ cloudiness=80.0, # 云量80%
+ precipitation=0.0,
+ precipitation_deposits=0.0,
+ wind_intensity=10.0,
+ fog_density=0.0,
+ fog_distance=0.0,
+ fog_falloff=1.0,
+ wetness=0.0,
+ scattering_intensity=0.1,
+ mie_scattering_scale=0.1,
+ rayleigh_scattering_scale=0.1
+ )
+
+ # 3. 小雨中午（少量降雨、路面微湿）
+ light_rain_noon = carla.WeatherParameters(
+ sun_altitude_angle=75.0,
+ sun_azimuth_angle=90.0,
+ cloudiness=90.0, # 云量90%
+ precipitation=20.0, # 降水量20%（小雨）
+ precipitation_deposits=5.0, # 路面雨水沉积5%
+ wind_intensity=15.0,
+ fog_density=5.0, # 轻微雾霭
+ fog_distance=50.0,
+ fog_falloff=0.8,
+ wetness=0.2, # 路面湿度20%
+ scattering_intensity=0.2,
+ mie_scattering_scale=0.2,
+ rayleigh_scattering_scale=0.2
+ )
+
+ # 4. 中雨中午（中等降雨、路面湿滑）
+ mid_rain_noon = carla.WeatherParameters(
+ sun_altitude_angle=75.0,
+ sun_azimuth_angle=90.0,
+ cloudiness=100.0, # 满云
+ precipitation=50.0, # 降水量50%（中雨）
+ precipitation_deposits=20.0, # 路面雨水沉积20%
+ wind_intensity=20.0,
+ fog_density=15.0, # 雾密度15%
+ fog_distance=30.0,
+ fog_falloff=0.6,
+ wetness=0.5, # 路面湿度50%
+ scattering_intensity=0.3,
+ mie_scattering_scale=0.3,
+ rayleigh_scattering_scale=0.3
+ )
+
+ # 5. 雾天中午（大雾、能见度低）
+ mist_noon = carla.WeatherParameters(
+ sun_altitude_angle=75.0,
+ sun_azimuth_angle=90.0,
+ cloudiness=50.0,
+ precipitation=0.0,
+ precipitation_deposits=0.0,
+ wind_intensity=5.0,
+ fog_density=30.0, # 雾密度30%（大雾）
+ fog_distance=10.0, # 雾的可见距离10米
+ fog_falloff=0.5,
+ wetness=0.1,
+ scattering_intensity=0.4,
+ mie_scattering_scale=0.4,
+ rayleigh_scattering_scale=0.4
+ )
+
+ # 6. 晴天日落（太阳低垂、暖色调、无云）
+ clear_sunset = carla.WeatherParameters(
+ sun_altitude_angle=15.0, # 太阳高度角15°（日落，地平线为0°）
+ sun_azimuth_angle=180.0, # 太阳方位角180°（西方）
+ cloudiness=0.0,
+ precipitation=0.0,
+ precipitation_deposits=0.0,
+ wind_intensity=5.0,
+ fog_density=0.0,
+ fog_distance=0.0,
+ fog_falloff=1.0,
+ wetness=0.0,
+ scattering_intensity=0.1,
+ mie_scattering_scale=0.1,
+ rayleigh_scattering_scale=0.1
+ )
+
+ # 7. 潮湿路面（无雨但路面湿滑、轻微雾）
+ wet_road = carla.WeatherParameters(
+ sun_altitude_angle=75.0,
+ sun_azimuth_angle=90.0,
+ cloudiness=30.0,
+ precipitation=0.0,
+ precipitation_deposits=0.0,
+ wind_intensity=10.0,
+ fog_density=5.0,
+ fog_distance=40.0,
+ fog_falloff=0.9,
+ wetness=0.8, # 路面湿度80%（湿滑）
+ scattering_intensity=0.1,
+ mie_scattering_scale=0.1,
+ rayleigh_scattering_scale=0.1
+ )
+
+ # 组合天气预设列表
+ weather_presets = [
+ ("Clear Noon", clear_noon),
+ ("Cloudy Noon", cloudy_noon),
+ ("Light Rain Noon", light_rain_noon),
+ ("Mid Rain Noon", mid_rain_noon),
+ ("Mist Noon", mist_noon),
+ ("Clear Sunset", clear_sunset),
+ ("Wet Road Noon", wet_road)
+ ]
+
+ return weather_presets
+
+
+def switch_weather(world: carla.World, weather: carla.WeatherParameters, weather_name: str) -> None:
+ """
+ 设置CARLA世界的天气，并打印切换信息
+
+ Args:
+ world (carla.World): CARLA模拟器的世界对象
+ weather (carla.WeatherParameters): 目标天气参数对象
+ weather_name (str): 天气名称，用于打印日志
+ """
+ try:
+ # 设置世界天气
+ world.set_weather(weather)
+ print(f"\n=== Switched to weather: {weather_name} ===")
+ except Exception as e:
+ print(f"Error switching weather to {weather_name}: {e}")
+
+
+def main() -> None:
+ """
+ 主执行函数：
+ 1. 连接CARLA服务器
+ 2. 清理旧车辆
+ 3. 生成特斯拉Model3车辆
+ 4. 设置旁观者相机
+ 5. 定时循环切换天气
+ 6. 保持仿真运行直到用户中断
+ """
+ # 初始化变量，避免finally块中引用未定义的变量
+ client: carla.Client = None
+ world: carla.World = None
+ vehicle: carla.Vehicle = None
+
+ # 天气相关变量初始化
+ weather_presets = get_weather_presets() # 获取天气预设列表
+ current_weather_index = 0 # 当前天气的索引
+ weather_switch_interval = 10 # 天气切换间隔（秒）
+ last_weather_switch_time = time.time() # 上一次天气切换的时间戳
+
+ try:
+ # 连接到本地CARLA服务器（地址：localhost，端口：2000）
+ client = carla.Client('localhost', 2000)
+ # 设置连接超时时间（10秒），避免无限等待
+ client.set_timeout(10.0)
+ print("Connecting to CARLA server...")
+
+ # 获取当前CARLA世界对象（包含地图、Actor、天气等信息）
+ world = client.get_world()
+ # 打印当前加载的地图名称
+ print(f"Connected to world: {world.get_map().name}")
+
+ # 清理之前运行残留的'my_car'车辆
+ remove_previous_vehicle(world)
+
+ # 获取地图的所有预设生成点（用于车辆/行人的生成）
+ spawn_points = world.get_map().get_spawn_points()
+ if not spawn_points:
+ print("Error: No spawn points found on the map!")
+ return
+ # 选择第一个生成点作为车辆生成位置
+ spawn_point = spawn_points[0]
+
+ # 获取蓝图库（包含所有可生成的Actor蓝图：车辆、行人、传感器等）
+ vehicle_bp_library = world.get_blueprint_library()
+ # 过滤出特斯拉Model3的蓝图（vehicle.tesla.model3 是CARLA中该车辆的唯一标识）
+ vehicle_bp = vehicle_bp_library.filter('vehicle.tesla.model3')[0]
+ # 设置车辆的角色名，方便后续清理
+ vehicle_bp.set_attribute('role_name', 'my_car')
+
+ # 尝试生成车辆（try_spawn_actor会检查生成点是否被占用，返回None表示失败）
+ print("Attempting to spawn vehicle...")
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+
+ if vehicle is None:
+ # 生成失败（可能生成点被占用）
+ print(f"Error: Failed to spawn vehicle at {spawn_point.location}.")
+ return
+
+ # 打印生成成功的车辆信息
+ print(f"Vehicle {vehicle.type_id} (ID {vehicle.id}) spawned successfully.")
+
+ # 等待车辆稳定（等待一次仿真tick，再加0.5秒延时）
+ world.wait_for_tick()
+ time.sleep(0.5)
+
+ # 设置旁观者相机到车辆后上方
+ set_spectator_behind_vehicle(world, vehicle)
+
+ # 初始化天气为第一个预设
+ switch_weather(world, weather_presets[0][1], weather_presets[0][0])
+
+ # 打印运行提示
+ print(f"\nSimulation running. Vehicle is stationary.")
+ print(f"Weather will switch every {weather_switch_interval} seconds.")
+ print("Press Ctrl+C to stop.\n")
+
+ # 主循环：保持仿真运行并定时切换天气
+ while True:
+ # 等待仿真tick（推进仿真时间）
+ world.wait_for_tick()
+
+ # 检查是否到达天气切换时间
+ current_time = time.time()
+ if current_time - last_weather_switch_time >= weather_switch_interval:
+ # 切换到下一个天气（循环遍历预设列表）
+ current_weather_index = (current_weather_index + 1) % len(weather_presets)
+ weather_name, weather = weather_presets[current_weather_index]
+ switch_weather(world, weather, weather_name)
+ # 更新上一次切换时间
+ last_weather_switch_time = current_time
+
+ # 小延时，避免循环过于频繁（减少CPU占用）
+ time.sleep(0.1)
+
+ except KeyboardInterrupt:
+ # 捕获用户Ctrl+C中断，友好退出
+ print("\nScript stopped by user (Ctrl+C).")
+ except Exception as e:
+ # 捕获其他未预期的异常，打印错误信息和堆栈跟踪
+ print(f"\nAn unexpected error occurred: {e}")
+ import traceback
+ traceback.print_exc()
+ finally:
+ # 资源清理：确保车辆被销毁，避免残留
+ print("\nStarting resource cleanup...")
+ if vehicle is not None and vehicle.is_alive:
+ print(f"Destroying vehicle: {vehicle.type_id} (ID {vehicle.id})")
+ if vehicle.destroy():
+ print("Vehicle destroyed successfully.")
+ else:
+ print("Vehicle destroy() returned False.")
+ else:
+ print("Vehicle was None or not alive, no destruction needed.")
+
+ print("Simulation finished.")
+
+
+# 程序入口
+if __name__ == '__main__':
+ main()
\ No newline at end of file
From 938de18944447a50c53835bff100eaafbea2f9e6 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Thu, 18 Dec 2025 22:09:40 +0800
Subject: [PATCH 02/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=AC=AC=E4=B8=80?=
=?UTF-8?q?=E4=BA=BA=E7=A7=B0=E8=A7=86=E8=A7=92?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/v1.py | 0
1 file changed, 0 insertions(+), 0 deletions(-)
create mode 100644 src/autonomous_driving_car/v1.py
diff --git a/src/autonomous_driving_car/v1.py b/src/autonomous_driving_car/v1.py
new file mode 100644
index 0000000000..e69de29bb2
From 5f2b01ea510d1b92e99d9847acbd5a102bed69a3 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Thu, 18 Dec 2025 22:27:28 +0800
Subject: [PATCH 03/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=AC=AC=E4=B8=80?=
=?UTF-8?q?=E8=A7=86=E8=A7=92?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/v1.py | 208 +++++++++++++++++++++++++++++++
1 file changed, 208 insertions(+)
diff --git a/src/autonomous_driving_car/v1.py b/src/autonomous_driving_car/v1.py
index e69de29bb2..8e65a90916 100644
--- a/src/autonomous_driving_car/v1.py
+++ b/src/autonomous_driving_car/v1.py
@@ -0,0 +1,208 @@
+#!/usr/bin/env python
+"""
+CARLA Vehicle Spawn with Pygame Display & Plotting (sd_3/__main__.py)
+
+This script connects to CARLA, spawns a vehicle, applies constant
+forward throttle, and visualizes the simulation using:
+1. PygameDisplay: Shows a rigidly attached camera view in a Pygame window.
+2. Plotter: Plots the vehicle's X-coordinate and speed vs. time using Matplotlib.
+
+It also sets the initial spectator camera position to a fixed viewpoint.
+
+Dependencies:
+- tools.pygame_display (Specifically the PygameDisplay class)
+- tools.plotter_x (Specifically the Plotter class for X-coord/Speed plots)
+"""
+
+import carla
+import numpy as np
+import time
+import pygame # Pygame is implicitly needed by PygameDisplay
+import math
+
+# Import helper classes from 'tools' directory using the specified format
+from tools.pygame_display import PygameDisplay
+from tools.plotter_x import Plotter
+
+# Global variable to store the simulation start time for plotting
+simulation_start_time = 0.0
+
+# --- Vehicle Data Functions ---
+def get_location(vehicle):
+ """Returns the current location of the vehicle."""
+ return vehicle.get_location()
+
+def get_speed_kmh(vehicle):
+ """Returns the current speed of the vehicle in km/h."""
+ velocity_vector = vehicle.get_velocity()
+ # Calculate the magnitude of the velocity vector (speed in m/s)
+ speed_meters_per_second = np.linalg.norm([velocity_vector.x, velocity_vector.y, velocity_vector.z])
+ return 3.6 * speed_meters_per_second
+
+# --- Simulation Management Functions ---
+def remove_previous_vehicle(world):
+ """Finds and removes all vehicles with the role 'my_car'."""
+ print("Searching for previous 'my_car' vehicles...")
+ actors = world.get_actors().filter('vehicle.*')
+ count = 0
+ for actor in actors:
+ if actor.attributes.get('role_name') == 'my_car':
+ print(f" - Removing previous vehicle: {actor.type_id} (ID {actor.id})")
+ if actor.destroy():
+ count += 1
+ else:
+ print(f" - Failed to remove vehicle {actor.id}")
+ print(f"Removed {count} previous vehicles.")
+
+
+# --- Spectator Setup ---
+def set_initial_spectator_view(world):
+ """Sets the spectator camera to a predefined fixed position and rotation."""
+ spectator = world.get_spectator()
+ # Define spectator transform using the desired fixed values (adjust as needed)
+ spectator_location = carla.Location(x=63.12, y=29.88, z=5.61)
+ spectator_rotation = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00)
+ spectator_transform = carla.Transform(spectator_location, spectator_rotation)
+ try:
+ spectator.set_transform(spectator_transform)
+ print(f"Initial spectator position set to: Loc=[{spectator_location.x:.2f}, {spectator_location.y:.2f}, {spectator_location.z:.2f}], Rot=[P:{spectator_rotation.pitch:.2f}, Y:{spectator_rotation.yaw:.2f}, R:{spectator_rotation.roll:.2f}]")
+ except Exception as e:
+ print(f"Error setting spectator transform: {e}")
+
+
+def main():
+ """Main execution function."""
+ global simulation_start_time # Allow modification of the global variable
+
+ client = None
+ world = None
+ vehicle = None
+ pygame_display = None
+ plotter = None
+
+ try:
+ # Connect to CARLA
+ client = carla.Client('localhost', 2000)
+ client.set_timeout(10.0)
+ print("Connecting to CARLA server...")
+ world = client.get_world()
+ print(f"Connected to world: {world.get_map().name}")
+
+ # Cleanup previous actors
+ remove_previous_vehicle(world)
+
+ # Get spawn point
+ spawn_points = world.get_map().get_spawn_points()
+ if not spawn_points:
+ print("Error: No spawn points found on the map!")
+ return
+ spawn_point = spawn_points[0] # Use the first spawn point
+ print(f"Selected spawn point: {spawn_point.location}")
+
+ # Get vehicle blueprint
+ vehicle_bp_library = world.get_blueprint_library()
+ vehicle_bp = vehicle_bp_library.filter('vehicle.tesla.model3')[0]
+ vehicle_bp.set_attribute('role_name', 'my_car')
+
+ # Spawn vehicle
+ print("Attempting to spawn vehicle...")
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+ if vehicle is None:
+ print(f"Error: Failed to spawn vehicle at {spawn_point.location}")
+ return
+ print(f"Vehicle {vehicle.type_id} (ID {vehicle.id}) spawned at {vehicle.get_location()}.")
+
+ # Set spectator view
+ set_initial_spectator_view(world)
+
+ # --- Initialize Plotter & Pygame ---
+ print("Initializing Plotter...")
+ plotter = Plotter() # Uses tools.plotter_x
+ plotter.init_plot()
+ print("Plotter initialized.")
+
+ print("Initializing Pygame display...")
+ pygame_display = PygameDisplay(world, vehicle) # Uses tools.pygame_display
+ print("Pygame display initialized.")
+
+ # Record the start time for the plot's time axis
+ simulation_start_time = time.time()
+
+ # --- Main Simulation Loop ---
+ print("Simulation running. Applying constant throttle. Press ESC or close Pygame window to stop.")
+ while True:
+ current_loop_time = time.time() # Get time at the start of the loop iteration
+
+ # Handle Pygame events (QUIT, ESC)
+ if pygame_display.parse_events():
+ print("Quit requested via Pygame window.")
+ break # Exit the loop if quit is requested
+
+ # Wait for the next simulation tick
+ world.wait_for_tick()
+
+ # --- Data Collection for Plotter ---
+ current_location = get_location(vehicle)
+ current_speed_kmh = get_speed_kmh(vehicle)
+ # Calculate time elapsed since simulation start
+ current_sim_time_sec = current_loop_time - simulation_start_time
+
+ # --- Render the Pygame display ---
+ pygame_display.render()
+
+ # --- Update Plot ---
+ if plotter is not None and plotter.is_initialized:
+ try:
+ # plotter_x expects time, x, current_speed, desired_speed
+ # We don't have a desired speed here, so pass 0 or current speed
+ plotter.update_plot(current_sim_time_sec, current_location.x, current_speed_kmh, 0.0)
+ except Exception as plot_update_e:
+ # Handle potential errors if the plot window was closed
+ print(f"Error updating plot (likely closed): {plot_update_e}")
+ plotter.cleanup_plot() # Attempt cleanup
+ plotter = None # Stop trying to update
+
+ # --- Vehicle Control ---
+ # Apply constant forward throttle
+ control = carla.VehicleControl(throttle=0.8, steer=0.0, brake=0.0)
+ vehicle.apply_control(control)
+
+ except KeyboardInterrupt:
+ print("\nScript interrupted by user (Ctrl+C).")
+ except Exception as e:
+ print(f"\nAn unexpected error occurred: {e}")
+ import traceback
+ traceback.print_exc()
+
+ finally:
+ print("Starting resource cleanup...")
+
+ # Destroy Pygame display first (handles its own camera cleanup)
+ if pygame_display is not None:
+ print("Destroying Pygame display...")
+ pygame_display.destroy()
+ print("Pygame display destroyed.")
+
+ # Cleanup plotter
+ if plotter is not None:
+ # Check if it needs cleanup (might already be None if closed/error)
+ if plotter.is_initialized:
+ print("Cleaning up plotter...")
+ plotter.cleanup_plot()
+ print("Plotter cleaned up.")
+
+ # Destroy the main vehicle
+ if vehicle is not None and vehicle.is_alive:
+ print(f"Destroying vehicle: {vehicle.type_id} (ID {vehicle.id})")
+ # vehicle.set_simulate_physics(False) # Optional: might help ensure clean removal
+ if vehicle.destroy():
+ print("Vehicle destroyed successfully.")
+ else:
+ print("Vehicle destroy() returned False.")
+ else:
+ print("Vehicle was None or not alive, no destruction needed.")
+
+ print("Simulation finished.")
+
+if __name__ == '__main__':
+ main()
From 6032ef7ce4754a2c660acde8ec5c51a43faeb8aa Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 10:37:05 +0800
Subject: [PATCH 04/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=BD=A6=E8=BE=86?=
=?UTF-8?q?=E7=8A=B6=E6=80=81=E4=BF=A1=E6=81=AF=E6=98=BE=E7=A4=BA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/v2.py | 257 +++++++++++++++++++++++++++++++
1 file changed, 257 insertions(+)
create mode 100644 src/autonomous_driving_car/v2.py
diff --git a/src/autonomous_driving_car/v2.py b/src/autonomous_driving_car/v2.py
new file mode 100644
index 0000000000..4e0a889bc9
--- /dev/null
+++ b/src/autonomous_driving_car/v2.py
@@ -0,0 +1,257 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+CARLA Vehicle Simulation with Visualization (sd_3/__main__.py)
+
+核心功能：
+1. 连接CARLA仿真服务器，清理历史车辆 Actor
+2. 生成特斯拉Model3车辆，施加恒定油门控制
+3. 基于PygameDisplay显示车辆挂载摄像头的实时画面
+4. 基于Plotter绘制车辆X坐标和速度随时间的变化曲线
+5. 设置固定的旁观者相机视角，方便观察仿真过程
+
+依赖：
+- tools.pygame_display.PygameDisplay：Pygame窗口显示摄像头画面
+- tools.plotter_x.Plotter：Matplotlib绘制X坐标/速度曲线
+"""
+
+# 加入这几行代码，解决模块导入问题
+import sys
+import os
+
+# 获取项目根目录（carla-python-examples）
+PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
+# 将项目根目录加入Python搜索路径
+sys.path.append(PROJECT_ROOT)
+import carla
+import numpy as np
+import time
+import pygame
+import math
+import traceback
+from typing import Optional, Tuple
+
+# 导入自定义工具类
+from tools.pygame_display import PygameDisplay
+from tools.plotter_x import Plotter
+
+# ===================== 配置集中化（便于修改和维护）=====================
+class SimConfig:
+ """仿真配置类：集中管理所有硬编码参数"""
+ # CARLA服务器连接信息
+ CARLA_HOST = "localhost"
+ CARLA_PORT = 2000
+ CARLA_TIMEOUT = 10.0 # 连接超时时间（秒）
+
+ # 车辆配置
+ VEHICLE_MODEL = "vehicle.tesla.model3" # 车辆模型
+ VEHICLE_ROLE_NAME = "my_car" # 车辆角色名（用于清理历史车辆）
+ CONSTANT_THROTTLE = 0.8 # 恒定油门值（0-1）
+ STEER_ANGLE = 0.0 # 恒定转向角（0=直线行驶）
+ BRAKE_VALUE = 0.0 # 刹车值（0=不刹车）
+
+ # 旁观者相机固定视角（可根据地图调整）
+ SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61)
+ SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00)
+
+ # 绘图配置
+ DESIRED_SPEED = 0.0 # 期望速度（若无速度控制，设为0）
+
+# ===================== 车辆状态计算工具函数 =====================
+def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float:
+ """
+ 计算车辆当前速度（km/h）
+ :param vehicle: CARLA车辆Actor对象
+ :return: 车辆速度（km/h）
+ """
+ velocity = vehicle.get_velocity()
+ # 计算速度矢量的模（m/s），转换为km/h（×3.6）
+ speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z])
+ return speed_mps * 3.6
+
+def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location:
+ """
+ 获取车辆当前位置
+ :param vehicle: CARLA车辆Actor对象
+ :return: 车辆的Location对象
+ """
+ return vehicle.get_location()
+
+# ===================== 仿真资源管理工具函数 =====================
+def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int:
+ """
+ 清理地图中指定角色名的历史车辆
+ :param world: CARLA的World对象
+ :param role_name: 车辆角色名
+ :return: 成功销毁的车辆数量
+ """
+ print(f"\n[资源清理] 搜索角色为'{role_name}'的历史车辆...")
+ vehicles = world.get_actors().filter("vehicle.*")
+ destroyed_count = 0
+
+ for vehicle in vehicles:
+ if vehicle.attributes.get("role_name") == role_name:
+ print(f" - 销毁历史车辆：{vehicle.type_id} (ID: {vehicle.id})")
+ if vehicle.destroy():
+ destroyed_count += 1
+ else:
+ print(f" - 销毁车辆{vehicle.id}失败")
+
+ print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆")
+ return destroyed_count
+
+def set_spectator_fixed_view(world: carla.World, location: carla.Location, rotation: carla.Rotation) -> bool:
+ """
+ 设置旁观者相机的固定视角
+ :param world: CARLA的World对象
+ :param location: 旁观者相机位置
+ :param rotation: 旁观者相机旋转角度
+ :return: 是否设置成功
+ """
+ try:
+ spectator = world.get_spectator()
+ spectator_transform = carla.Transform(location, rotation)
+ spectator.set_transform(spectator_transform)
+ print(f"\n[视角设置] 旁观者相机位置：({location.x:.2f}, {location.y:.2f}, {location.z:.2f})")
+ print(f"[视角设置] 旁观者相机旋转：(俯仰：{rotation.pitch:.2f}, 偏航：{rotation.yaw:.2f}, 翻滚：{rotation.roll:.2f})")
+ return True
+ except Exception as e:
+ print(f"[视角设置] 失败：{e}")
+ return False
+
+# ===================== 主仿真函数 =====================
+def main():
+ """主仿真入口函数：完成所有仿真流程的初始化、运行和清理"""
+ # 初始化核心变量（所有资源对象初始化为None，便于后续清理）
+ client: Optional[carla.Client] = None
+ world: Optional[carla.World] = None
+ vehicle: Optional[carla.Vehicle] = None
+ pygame_display: Optional[PygameDisplay] = None
+ plotter: Optional[Plotter] = None
+ simulation_start_time: Optional[float] = None
+
+ try:
+ # 1. 连接CARLA服务器
+ print(f"[CARLA连接] 尝试连接 {SimConfig.CARLA_HOST}:{SimConfig.CARLA_PORT}...")
+ client = carla.Client(SimConfig.CARLA_HOST, SimConfig.CARLA_PORT)
+ client.set_timeout(SimConfig.CARLA_TIMEOUT)
+ world = client.get_world()
+ print(f"[CARLA连接] 成功连接到地图：{world.get_map().name}")
+
+ # 2. 清理历史车辆
+ clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME)
+
+ # 3. 获取车辆生成点
+ spawn_points = world.get_map().get_spawn_points()
+ if not spawn_points:
+ raise RuntimeError("[生成车辆] 地图中未找到可用的生成点！")
+ spawn_point = spawn_points[0]
+ print(f"[生成车辆] 选择生成点：({spawn_point.location.x:.2f}, {spawn_point.location.y:.2f}, {spawn_point.location.z:.2f})")
+
+ # 4. 加载车辆蓝图并设置属性
+ blueprint_library = world.get_blueprint_library()
+ vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0]
+ vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME)
+ print(f"[生成车辆] 加载车辆蓝图：{vehicle_bp.id}")
+
+ # 5. 生成车辆
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+ if vehicle is None:
+ raise RuntimeError(f"[生成车辆] 在生成点{spawn_point.location}生成车辆失败！")
+ print(f"[生成车辆] 成功生成：{vehicle.type_id} (ID: {vehicle.id})")
+
+ # 6. 设置旁观者相机视角
+ set_spectator_fixed_view(world, SimConfig.SPECTATOR_LOCATION, SimConfig.SPECTATOR_ROTATION)
+
+ # 7. 初始化可视化组件
+ print("\n[可视化] 初始化绘图器（Plotter）...")
+ plotter = Plotter()
+ plotter.init_plot()
+ print("[可视化] 绘图器初始化完成")
+
+ print("[可视化] 初始化Pygame显示窗口...")
+ pygame_display = PygameDisplay(world, vehicle)
+ print("[可视化] Pygame显示窗口初始化完成")
+
+ # 8. 记录仿真开始时间
+ simulation_start_time = time.time()
+ print(f"\n[仿真启动] 开始运行仿真（恒定油门：{SimConfig.CONSTANT_THROTTLE}）")
+ print("[仿真启动] 按ESC或关闭Pygame窗口停止仿真...")
+
+ # 9. 主仿真循环
+ while True:
+ # 9.1 处理Pygame事件（关闭窗口、ESC键）
+ if pygame_display.parse_events():
+ print("[仿真控制] 检测到Pygame退出请求，停止仿真...")
+ break
+
+ # 9.2 等待仿真tick（同步仿真时间）
+ world.wait_for_tick()
+
+ # 9.3 收集车辆状态数据
+ current_time = time.time()
+ vehicle_location = get_vehicle_location(vehicle)
+ vehicle_speed = calculate_vehicle_speed_kmh(vehicle)
+ sim_elapsed_time = current_time - simulation_start_time # 仿真已运行时间（秒）
+
+ # 9.4 渲染Pygame窗口（摄像头画面）
+ pygame_display.render()
+
+ # 9.5 更新绘图器（X坐标、速度曲线）
+ if plotter and plotter.is_initialized:
+ try:
+ plotter.update_plot(
+ sim_elapsed_time,
+ vehicle_location.x,
+ vehicle_speed,
+ SimConfig.DESIRED_SPEED
+ )
+ except Exception as e:
+ print(f"[绘图器] 更新失败（可能已关闭窗口）：{e}")
+ plotter.cleanup_plot()
+ plotter = None
+
+ # 9.6 车辆控制：施加恒定油门
+ vehicle_control = carla.VehicleControl(
+ throttle=SimConfig.CONSTANT_THROTTLE,
+ steer=SimConfig.STEER_ANGLE,
+ brake=SimConfig.BRAKE_VALUE
+ )
+ vehicle.apply_control(vehicle_control)
+
+ # 异常处理
+ except KeyboardInterrupt:
+ print("\n[仿真中断] 用户按下Ctrl+C，停止仿真...")
+ except RuntimeError as e:
+ print(f"\n[仿真错误] 运行时错误：{e}")
+ traceback.print_exc()
+ except Exception as e:
+ print(f"\n[仿真错误] 未知异常：{e}")
+ traceback.print_exc()
+
+ # 资源清理（无论是否异常，都执行）
+ finally:
+ print("\n[资源清理] 开始清理仿真资源...")
+
+ # 清理Pygame显示窗口
+ if pygame_display:
+ print("[资源清理] 销毁Pygame显示窗口...")
+ pygame_display.destroy()
+
+ # 清理绘图器
+ if plotter and plotter.is_initialized:
+ print("[资源清理] 清理绘图器...")
+ plotter.cleanup_plot()
+
+ # 销毁车辆
+ if vehicle and vehicle.is_alive:
+ print(f"[资源清理] 销毁车辆：{vehicle.type_id} (ID: {vehicle.id})")
+ if vehicle.destroy():
+ print("[资源清理] 车辆销毁成功")
+ else:
+ print("[资源清理] 车辆销毁失败")
+
+ print("[资源清理] 仿真资源清理完成")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From 2218a0f225e93e4423d9514032857b43e5a1a218 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 14:39:50 +0800
Subject: [PATCH 05/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E4=B8=BB?=
=?UTF-8?q?=E6=8E=A7=E5=88=B6=E4=B8=8E=E8=B7=AF=E5=BE=84=E8=B7=9F=E9=9A=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/v3.py | 344 +++++++++++++++++++++++++++++++
1 file changed, 344 insertions(+)
create mode 100644 src/autonomous_driving_car/v3.py
diff --git a/src/autonomous_driving_car/v3.py b/src/autonomous_driving_car/v3.py
new file mode 100644
index 0000000000..174d1de37d
--- /dev/null
+++ b/src/autonomous_driving_car/v3.py
@@ -0,0 +1,344 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+CARLA Waypoint Following with Simple Speed Control & Pygame Display (sd_4/__main__.py)
+
+核心功能：
+1. 连接CARLA仿真服务器，清理历史车辆Actor
+2. 生成特斯拉Model3车辆，基于预定义航点实现路径跟随
+3. 采用简易的bang-bang速度控制器，根据当前速度与目标速度调整油门/刹车
+4. 基于PygameDisplay显示车辆挂载摄像头的实时画面
+5. 基于Plotter绘制车辆X坐标、实际速度与期望速度的时间曲线
+6. 设置固定的旁观者相机视角，方便观察仿真过程
+
+依赖：
+- tools.plotter_x.Plotter：Matplotlib绘制X坐标/速度曲线
+- tools.pygame_display.PygameDisplay：Pygame窗口显示摄像头画面
+"""
+
+# ===================== 路径处理（解决tools模块导入问题）=====================
+import sys
+import os
+PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
+sys.path.append(PROJECT_ROOT)
+# ==============================================================================
+
+import carla
+import numpy as np
+import time
+import traceback
+from typing import Optional, List, Tuple
+
+# 导入自定义工具类
+from tools.plotter_x import Plotter
+from tools.pygame_display import PygameDisplay
+
+# ===================== 配置集中化（便于修改和维护）=====================
+class SimConfig:
+ """仿真配置类：集中管理所有硬编码参数"""
+ # CARLA服务器连接信息
+ CARLA_HOST = "localhost"
+ CARLA_PORT = 2000
+ CARLA_TIMEOUT = 10.0 # 连接超时时间（秒）
+
+ # 车辆配置
+ VEHICLE_MODEL = "vehicle.tesla.model3"
+ VEHICLE_ROLE_NAME = "my_car"
+
+ # 航点配置：[x坐标, y坐标, 期望速度(km/h)]
+ WAYPOINTS: List[List[float]] = [
+ [-64.0, 24.5, 40.0], # 起始点（第一段路径的参考点）
+ [40.0, 24.5, 40.0], # 制动点前的行驶点（保持40km/h）
+ [70.0, 24.5, 0.0] # 停止点（目标速度0km/h）
+ ]
+ WAYPOINT_THRESHOLD = 5.0 # 切换航点的距离阈值（米）
+ INITIAL_TARGET_WAYPOINT_ID = 1 # 初始目标航点索引（从1开始，0为起始点）
+
+ # 旁观者相机固定视角
+ SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61)
+ SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00)
+
+# ===================== 车辆状态计算工具函数 =====================
+def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location:
+ """
+ 获取车辆当前位置
+ :param vehicle: CARLA车辆Actor对象
+ :return: 车辆的Location对象
+ """
+ return vehicle.get_location()
+
+def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float:
+ """
+ 计算车辆当前速度（km/h）
+ :param vehicle: CARLA车辆Actor对象
+ :return: 车辆速度（km/h）
+ """
+ velocity = vehicle.get_velocity()
+ speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z])
+ return speed_mps * 3.6
+
+# ===================== 仿真资源管理工具函数 =====================
+def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int:
+ """
+ 清理地图中指定角色名的历史车辆
+ :param world: CARLA的World对象
+ :param role_name: 车辆角色名
+ :return: 成功销毁的车辆数量
+ """
+ print(f"\n[资源清理] 搜索角色为'{role_name}'的历史车辆...")
+ vehicles = world.get_actors().filter("vehicle.*")
+ destroyed_count = 0
+
+ for vehicle in vehicles:
+ if vehicle.attributes.get("role_name") == role_name:
+ print(f" - 销毁历史车辆：{vehicle.type_id} (ID: {vehicle.id})")
+ if vehicle.destroy():
+ destroyed_count += 1
+ else:
+ print(f" - 销毁车辆{vehicle.id}失败")
+
+ print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆")
+ return destroyed_count
+
+def set_spectator_fixed_view(world: carla.World) -> bool:
+ """
+ 设置旁观者相机的固定视角
+ :param world: CARLA的World对象
+ :return: 是否设置成功
+ """
+ try:
+ spectator = world.get_spectator()
+ spectator_transform = carla.Transform(
+ SimConfig.SPECTATOR_LOCATION,
+ SimConfig.SPECTATOR_ROTATION
+ )
+ spectator.set_transform(spectator_transform)
+ print(f"\n[视角设置] 旁观者相机位置：({SimConfig.SPECTATOR_LOCATION.x:.2f}, {SimConfig.SPECTATOR_LOCATION.y:.2f}, {SimConfig.SPECTATOR_LOCATION.z:.2f})")
+ print(f"[视角设置] 旁观者相机旋转：(俯仰：{SimConfig.SPECTATOR_ROTATION.pitch:.2f}, 偏航：{SimConfig.SPECTATOR_ROTATION.yaw:.2f}, 翻滚：{SimConfig.SPECTATOR_ROTATION.roll:.2f})")
+ return True
+ except Exception as e:
+ print(f"[视角设置] 失败：{e}")
+ return False
+
+# ===================== 航点管理函数 =====================
+def update_target_waypoint(
+ vehicle_location: carla.Location,
+ current_target_id: int,
+ waypoints: List[List[float]],
+ threshold: float
+) -> int:
+ """
+ 检查是否到达当前目标航点，若到达则切换到下一个航点
+ :param vehicle_location: 车辆当前位置
+ :param current_target_id: 当前目标航点索引
+ :param waypoints: 航点列表
+ :param threshold: 切换航点的距离阈值
+ :return: 更新后的目标航点索引
+ """
+ # 若已是最后一个航点，不再切换
+ if current_target_id >= len(waypoints) - 1:
+ return current_target_id
+
+ # 获取当前目标航点的位置
+ target_wp_data = waypoints[current_target_id]
+ target_loc = carla.Location(x=target_wp_data[0], y=target_wp_data[1])
+
+ # 关键修改：手动计算2D距离（替代CARLA低版本不存在的distance_2d方法）
+ dx = vehicle_location.x - target_loc.x
+ dy = vehicle_location.y - target_loc.y
+ distance = np.sqrt(dx ** 2 + dy ** 2)
+
+ # 若距离小于阈值，切换到下一个航点
+ if distance < threshold:
+ print(f"[航点更新] 到达航点{current_target_id}（距离：{distance:.1f}m），新目标航点：{current_target_id + 1}")
+ return current_target_id + 1
+
+ return current_target_id
+
+# ===================== 速度控制器 =====================
+def simple_speed_controller(v_desired: float, v_current: float) -> carla.VehicleControl:
+ """
+ 简易的3状态速度控制器（bang-bang控制）
+ 根据当前速度与期望速度的差值，全量施加油门或刹车
+ :param v_desired: 期望速度（km/h）
+ :param v_current: 当前速度（km/h）
+ :return: CARLA车辆控制指令
+ """
+ control = carla.VehicleControl()
+ control.steer = 0.0 # 暂不控制转向
+ control.throttle = 0.0
+ control.brake = 0.0
+
+ # 目标速度为0时，全力刹车
+ if v_desired == 0:
+ control.brake = 1.0
+ # 当前速度小于期望速度，全力加速
+ elif v_current < v_desired:
+ control.throttle = 1.0
+ # 当前速度大于期望速度，全力刹车
+ elif v_current > v_desired:
+ control.brake = 1.0
+
+ return control
+
+# ===================== 主仿真函数 =====================
+def main():
+ """主仿真入口函数：完成所有仿真流程的初始化、运行和清理"""
+ # 初始化核心变量
+ client: Optional[carla.Client] = None
+ world: Optional[carla.World] = None
+ vehicle: Optional[carla.Vehicle] = None
+ plotter: Optional[Plotter] = None
+ pygame_display: Optional[PygameDisplay] = None
+ simulation_start_time: Optional[float] = None
+ target_waypoint_id: int = SimConfig.INITIAL_TARGET_WAYPOINT_ID
+
+ try:
+ # 1. 连接CARLA服务器
+ print(f"[CARLA连接] 尝试连接 {SimConfig.CARLA_HOST}:{SimConfig.CARLA_PORT}...")
+ client = carla.Client(SimConfig.CARLA_HOST, SimConfig.CARLA_PORT)
+ client.set_timeout(SimConfig.CARLA_TIMEOUT)
+ world = client.get_world()
+ print(f"[CARLA连接] 成功连接到地图：{world.get_map().name}")
+
+ # 2. 清理历史车辆
+ clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME)
+
+ # 3. 生成车辆（基于第一个航点作为起始位置）
+ map_spawn_points = world.get_map().get_spawn_points()
+ if not map_spawn_points:
+ raise RuntimeError("[生成车辆] 地图中未找到可用的生成点！")
+
+ # 获取起始航点的位置
+ start_waypoint = SimConfig.WAYPOINTS[0]
+ # 关键修改：将z轴高度从0.5提高到1.5，避免与地面碰撞
+ start_location = carla.Location(x=start_waypoint[0], y=start_waypoint[1], z=1.5)
+
+ # 找到距离起始位置最近的地图生成点（用于获取道路朝向）
+ spawn_point = min(map_spawn_points, key=lambda sp: sp.location.distance(start_location))
+ spawn_point.location = start_location # 覆盖为自定义起始位置
+
+ print(f"[生成车辆] 目标生成位置：{start_location}，使用地图朝向：{spawn_point.rotation}")
+
+ # 加载车辆蓝图
+ blueprint_library = world.get_blueprint_library()
+ vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0]
+ vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME)
+
+ # 生成车辆
+ print("[生成车辆] 尝试生成车辆...")
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+
+ # 关键修改：添加重试逻辑，第一个点失败则尝试第二个地图默认点
+ if vehicle is None:
+ print("[生成车辆] 第一个点生成失败，尝试第二个地图默认点...")
+ spawn_point = map_spawn_points[1]
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+ if vehicle is None:
+ raise RuntimeError(f"[生成车辆] 生成失败，请检查坐标是否合法！")
+
+ print(f"[生成车辆] 成功生成：{vehicle.type_id} (ID: {vehicle.id})")
+
+ # 4. 设置旁观者相机视角
+ set_spectator_fixed_view(world)
+
+ # 5. 初始化可视化组件
+ print("\n[可视化] 初始化绘图器（Plotter）...")
+ plotter = Plotter()
+ plotter.init_plot()
+ print("[可视化] 绘图器初始化完成")
+
+ print("[可视化] 初始化Pygame显示窗口...")
+ pygame_display = PygameDisplay(world, vehicle)
+ print("[可视化] Pygame显示窗口初始化完成")
+
+ # 6. 记录仿真开始时间
+ simulation_start_time = time.time()
+ target_waypoint_id = SimConfig.INITIAL_TARGET_WAYPOINT_ID
+
+ # 7. 主仿真循环
+ print("\n[仿真启动] 开始航点跟随与速度控制。按ESC或关闭Pygame窗口停止仿真...")
+ while True:
+ current_loop_time = time.time()
+
+ # 处理Pygame事件（关闭窗口、ESC键）
+ if pygame_display.parse_events():
+ print("[仿真控制] 检测到Pygame退出请求，停止仿真...")
+ break
+
+ # 等待仿真tick（同步仿真时间）
+ world.wait_for_tick()
+
+ # 8. 收集车辆状态数据
+ vehicle_location = get_vehicle_location(vehicle)
+ vehicle_speed = calculate_vehicle_speed_kmh(vehicle)
+ sim_elapsed_time = current_loop_time - simulation_start_time
+
+ # 9. 更新可视化
+ # 更新绘图器
+ if plotter and plotter.is_initialized:
+ current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2]
+ try:
+ plotter.update_plot(sim_elapsed_time, vehicle_location.x, vehicle_speed, current_desired_speed)
+ except Exception as e:
+ print(f"[绘图器] 更新失败（可能已关闭窗口）：{e}")
+ plotter.cleanup_plot()
+ plotter = None
+
+ # 渲染Pygame窗口
+ pygame_display.render()
+
+ # 10. 航点跟随逻辑
+ # 更新目标航点
+ target_waypoint_id = update_target_waypoint(
+ vehicle_location,
+ target_waypoint_id,
+ SimConfig.WAYPOINTS,
+ SimConfig.WAYPOINT_THRESHOLD
+ )
+
+ # 获取当前目标航点的期望速度
+ current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] if 0 <= target_waypoint_id < len(SimConfig.WAYPOINTS) else 0.0
+
+ # 计算车辆控制指令
+ control = simple_speed_controller(current_desired_speed, vehicle_speed)
+
+ # 应用控制指令
+ vehicle.apply_control(control)
+
+ # 异常处理
+ except KeyboardInterrupt:
+ print("\n[仿真中断] 用户按下Ctrl+C，停止仿真...")
+ except RuntimeError as e:
+ print(f"\n[仿真错误] 运行时错误：{e}")
+ traceback.print_exc()
+ except Exception as e:
+ print(f"\n[仿真错误] 未知异常：{e}")
+ traceback.print_exc()
+
+ # 资源清理
+ finally:
+ print("\n[资源清理] 开始清理仿真资源...")
+
+ # 清理Pygame显示窗口
+ if pygame_display:
+ print("[资源清理] 销毁Pygame显示窗口...")
+ pygame_display.destroy()
+
+ # 清理绘图器
+ if plotter and plotter.is_initialized:
+ print("[资源清理] 清理绘图器...")
+ plotter.cleanup_plot()
+
+ # 销毁车辆
+ if vehicle and vehicle.is_alive:
+ print(f"[资源清理] 销毁车辆：{vehicle.type_id} (ID: {vehicle.id})")
+ if vehicle.destroy():
+ print("[资源清理] 车辆销毁成功")
+ else:
+ print("[资源清理] 车辆销毁失败")
+
+ print("[资源清理] 仿真资源清理完成")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From b1a8426883cbeb93406fe2603418e48541f97ee9 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 16:05:15 +0800
Subject: [PATCH 06/26] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E4=BA=86=E5=A4=9A?=
=?UTF-8?q?=E4=B8=AA=E5=85=A5=E5=8F=A3=E8=84=9A=E6=9C=AC=E9=97=AE=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/{ => FPV}/v1.py | 0
src/autonomous_driving_car/{ => MCP}/v2.py | 0
src/autonomous_driving_car/v3.py | 344 ---------------------
3 files changed, 344 deletions(-)
rename src/autonomous_driving_car/{ => FPV}/v1.py (100%)
rename src/autonomous_driving_car/{ => MCP}/v2.py (100%)
delete mode 100644 src/autonomous_driving_car/v3.py
diff --git a/src/autonomous_driving_car/v1.py b/src/autonomous_driving_car/FPV/v1.py
similarity index 100%
rename from src/autonomous_driving_car/v1.py
rename to src/autonomous_driving_car/FPV/v1.py
diff --git a/src/autonomous_driving_car/v2.py b/src/autonomous_driving_car/MCP/v2.py
similarity index 100%
rename from src/autonomous_driving_car/v2.py
rename to src/autonomous_driving_car/MCP/v2.py
diff --git a/src/autonomous_driving_car/v3.py b/src/autonomous_driving_car/v3.py
deleted file mode 100644
index 174d1de37d..0000000000
--- a/src/autonomous_driving_car/v3.py
+++ /dev/null
@@ -1,344 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-"""
-CARLA Waypoint Following with Simple Speed Control & Pygame Display (sd_4/__main__.py)
-
-核心功能：
-1. 连接CARLA仿真服务器，清理历史车辆Actor
-2. 生成特斯拉Model3车辆，基于预定义航点实现路径跟随
-3. 采用简易的bang-bang速度控制器，根据当前速度与目标速度调整油门/刹车
-4. 基于PygameDisplay显示车辆挂载摄像头的实时画面
-5. 基于Plotter绘制车辆X坐标、实际速度与期望速度的时间曲线
-6. 设置固定的旁观者相机视角，方便观察仿真过程
-
-依赖：
-- tools.plotter_x.Plotter：Matplotlib绘制X坐标/速度曲线
-- tools.pygame_display.PygameDisplay：Pygame窗口显示摄像头画面
-"""
-
-# ===================== 路径处理（解决tools模块导入问题）=====================
-import sys
-import os
-PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
-sys.path.append(PROJECT_ROOT)
-# ==============================================================================
-
-import carla
-import numpy as np
-import time
-import traceback
-from typing import Optional, List, Tuple
-
-# 导入自定义工具类
-from tools.plotter_x import Plotter
-from tools.pygame_display import PygameDisplay
-
-# ===================== 配置集中化（便于修改和维护）=====================
-class SimConfig:
- """仿真配置类：集中管理所有硬编码参数"""
- # CARLA服务器连接信息
- CARLA_HOST = "localhost"
- CARLA_PORT = 2000
- CARLA_TIMEOUT = 10.0 # 连接超时时间（秒）
-
- # 车辆配置
- VEHICLE_MODEL = "vehicle.tesla.model3"
- VEHICLE_ROLE_NAME = "my_car"
-
- # 航点配置：[x坐标, y坐标, 期望速度(km/h)]
- WAYPOINTS: List[List[float]] = [
- [-64.0, 24.5, 40.0], # 起始点（第一段路径的参考点）
- [40.0, 24.5, 40.0], # 制动点前的行驶点（保持40km/h）
- [70.0, 24.5, 0.0] # 停止点（目标速度0km/h）
- ]
- WAYPOINT_THRESHOLD = 5.0 # 切换航点的距离阈值（米）
- INITIAL_TARGET_WAYPOINT_ID = 1 # 初始目标航点索引（从1开始，0为起始点）
-
- # 旁观者相机固定视角
- SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61)
- SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00)
-
-# ===================== 车辆状态计算工具函数 =====================
-def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location:
- """
- 获取车辆当前位置
- :param vehicle: CARLA车辆Actor对象
- :return: 车辆的Location对象
- """
- return vehicle.get_location()
-
-def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float:
- """
- 计算车辆当前速度（km/h）
- :param vehicle: CARLA车辆Actor对象
- :return: 车辆速度（km/h）
- """
- velocity = vehicle.get_velocity()
- speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z])
- return speed_mps * 3.6
-
-# ===================== 仿真资源管理工具函数 =====================
-def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int:
- """
- 清理地图中指定角色名的历史车辆
- :param world: CARLA的World对象
- :param role_name: 车辆角色名
- :return: 成功销毁的车辆数量
- """
- print(f"\n[资源清理] 搜索角色为'{role_name}'的历史车辆...")
- vehicles = world.get_actors().filter("vehicle.*")
- destroyed_count = 0
-
- for vehicle in vehicles:
- if vehicle.attributes.get("role_name") == role_name:
- print(f" - 销毁历史车辆：{vehicle.type_id} (ID: {vehicle.id})")
- if vehicle.destroy():
- destroyed_count += 1
- else:
- print(f" - 销毁车辆{vehicle.id}失败")
-
- print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆")
- return destroyed_count
-
-def set_spectator_fixed_view(world: carla.World) -> bool:
- """
- 设置旁观者相机的固定视角
- :param world: CARLA的World对象
- :return: 是否设置成功
- """
- try:
- spectator = world.get_spectator()
- spectator_transform = carla.Transform(
- SimConfig.SPECTATOR_LOCATION,
- SimConfig.SPECTATOR_ROTATION
- )
- spectator.set_transform(spectator_transform)
- print(f"\n[视角设置] 旁观者相机位置：({SimConfig.SPECTATOR_LOCATION.x:.2f}, {SimConfig.SPECTATOR_LOCATION.y:.2f}, {SimConfig.SPECTATOR_LOCATION.z:.2f})")
- print(f"[视角设置] 旁观者相机旋转：(俯仰：{SimConfig.SPECTATOR_ROTATION.pitch:.2f}, 偏航：{SimConfig.SPECTATOR_ROTATION.yaw:.2f}, 翻滚：{SimConfig.SPECTATOR_ROTATION.roll:.2f})")
- return True
- except Exception as e:
- print(f"[视角设置] 失败：{e}")
- return False
-
-# ===================== 航点管理函数 =====================
-def update_target_waypoint(
- vehicle_location: carla.Location,
- current_target_id: int,
- waypoints: List[List[float]],
- threshold: float
-) -> int:
- """
- 检查是否到达当前目标航点，若到达则切换到下一个航点
- :param vehicle_location: 车辆当前位置
- :param current_target_id: 当前目标航点索引
- :param waypoints: 航点列表
- :param threshold: 切换航点的距离阈值
- :return: 更新后的目标航点索引
- """
- # 若已是最后一个航点，不再切换
- if current_target_id >= len(waypoints) - 1:
- return current_target_id
-
- # 获取当前目标航点的位置
- target_wp_data = waypoints[current_target_id]
- target_loc = carla.Location(x=target_wp_data[0], y=target_wp_data[1])
-
- # 关键修改：手动计算2D距离（替代CARLA低版本不存在的distance_2d方法）
- dx = vehicle_location.x - target_loc.x
- dy = vehicle_location.y - target_loc.y
- distance = np.sqrt(dx ** 2 + dy ** 2)
-
- # 若距离小于阈值，切换到下一个航点
- if distance < threshold:
- print(f"[航点更新] 到达航点{current_target_id}（距离：{distance:.1f}m），新目标航点：{current_target_id + 1}")
- return current_target_id + 1
-
- return current_target_id
-
-# ===================== 速度控制器 =====================
-def simple_speed_controller(v_desired: float, v_current: float) -> carla.VehicleControl:
- """
- 简易的3状态速度控制器（bang-bang控制）
- 根据当前速度与期望速度的差值，全量施加油门或刹车
- :param v_desired: 期望速度（km/h）
- :param v_current: 当前速度（km/h）
- :return: CARLA车辆控制指令
- """
- control = carla.VehicleControl()
- control.steer = 0.0 # 暂不控制转向
- control.throttle = 0.0
- control.brake = 0.0
-
- # 目标速度为0时，全力刹车
- if v_desired == 0:
- control.brake = 1.0
- # 当前速度小于期望速度，全力加速
- elif v_current < v_desired:
- control.throttle = 1.0
- # 当前速度大于期望速度，全力刹车
- elif v_current > v_desired:
- control.brake = 1.0
-
- return control
-
-# ===================== 主仿真函数 =====================
-def main():
- """主仿真入口函数：完成所有仿真流程的初始化、运行和清理"""
- # 初始化核心变量
- client: Optional[carla.Client] = None
- world: Optional[carla.World] = None
- vehicle: Optional[carla.Vehicle] = None
- plotter: Optional[Plotter] = None
- pygame_display: Optional[PygameDisplay] = None
- simulation_start_time: Optional[float] = None
- target_waypoint_id: int = SimConfig.INITIAL_TARGET_WAYPOINT_ID
-
- try:
- # 1. 连接CARLA服务器
- print(f"[CARLA连接] 尝试连接 {SimConfig.CARLA_HOST}:{SimConfig.CARLA_PORT}...")
- client = carla.Client(SimConfig.CARLA_HOST, SimConfig.CARLA_PORT)
- client.set_timeout(SimConfig.CARLA_TIMEOUT)
- world = client.get_world()
- print(f"[CARLA连接] 成功连接到地图：{world.get_map().name}")
-
- # 2. 清理历史车辆
- clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME)
-
- # 3. 生成车辆（基于第一个航点作为起始位置）
- map_spawn_points = world.get_map().get_spawn_points()
- if not map_spawn_points:
- raise RuntimeError("[生成车辆] 地图中未找到可用的生成点！")
-
- # 获取起始航点的位置
- start_waypoint = SimConfig.WAYPOINTS[0]
- # 关键修改：将z轴高度从0.5提高到1.5，避免与地面碰撞
- start_location = carla.Location(x=start_waypoint[0], y=start_waypoint[1], z=1.5)
-
- # 找到距离起始位置最近的地图生成点（用于获取道路朝向）
- spawn_point = min(map_spawn_points, key=lambda sp: sp.location.distance(start_location))
- spawn_point.location = start_location # 覆盖为自定义起始位置
-
- print(f"[生成车辆] 目标生成位置：{start_location}，使用地图朝向：{spawn_point.rotation}")
-
- # 加载车辆蓝图
- blueprint_library = world.get_blueprint_library()
- vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0]
- vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME)
-
- # 生成车辆
- print("[生成车辆] 尝试生成车辆...")
- vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
-
- # 关键修改：添加重试逻辑，第一个点失败则尝试第二个地图默认点
- if vehicle is None:
- print("[生成车辆] 第一个点生成失败，尝试第二个地图默认点...")
- spawn_point = map_spawn_points[1]
- vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
- if vehicle is None:
- raise RuntimeError(f"[生成车辆] 生成失败，请检查坐标是否合法！")
-
- print(f"[生成车辆] 成功生成：{vehicle.type_id} (ID: {vehicle.id})")
-
- # 4. 设置旁观者相机视角
- set_spectator_fixed_view(world)
-
- # 5. 初始化可视化组件
- print("\n[可视化] 初始化绘图器（Plotter）...")
- plotter = Plotter()
- plotter.init_plot()
- print("[可视化] 绘图器初始化完成")
-
- print("[可视化] 初始化Pygame显示窗口...")
- pygame_display = PygameDisplay(world, vehicle)
- print("[可视化] Pygame显示窗口初始化完成")
-
- # 6. 记录仿真开始时间
- simulation_start_time = time.time()
- target_waypoint_id = SimConfig.INITIAL_TARGET_WAYPOINT_ID
-
- # 7. 主仿真循环
- print("\n[仿真启动] 开始航点跟随与速度控制。按ESC或关闭Pygame窗口停止仿真...")
- while True:
- current_loop_time = time.time()
-
- # 处理Pygame事件（关闭窗口、ESC键）
- if pygame_display.parse_events():
- print("[仿真控制] 检测到Pygame退出请求，停止仿真...")
- break
-
- # 等待仿真tick（同步仿真时间）
- world.wait_for_tick()
-
- # 8. 收集车辆状态数据
- vehicle_location = get_vehicle_location(vehicle)
- vehicle_speed = calculate_vehicle_speed_kmh(vehicle)
- sim_elapsed_time = current_loop_time - simulation_start_time
-
- # 9. 更新可视化
- # 更新绘图器
- if plotter and plotter.is_initialized:
- current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2]
- try:
- plotter.update_plot(sim_elapsed_time, vehicle_location.x, vehicle_speed, current_desired_speed)
- except Exception as e:
- print(f"[绘图器] 更新失败（可能已关闭窗口）：{e}")
- plotter.cleanup_plot()
- plotter = None
-
- # 渲染Pygame窗口
- pygame_display.render()
-
- # 10. 航点跟随逻辑
- # 更新目标航点
- target_waypoint_id = update_target_waypoint(
- vehicle_location,
- target_waypoint_id,
- SimConfig.WAYPOINTS,
- SimConfig.WAYPOINT_THRESHOLD
- )
-
- # 获取当前目标航点的期望速度
- current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] if 0 <= target_waypoint_id < len(SimConfig.WAYPOINTS) else 0.0
-
- # 计算车辆控制指令
- control = simple_speed_controller(current_desired_speed, vehicle_speed)
-
- # 应用控制指令
- vehicle.apply_control(control)
-
- # 异常处理
- except KeyboardInterrupt:
- print("\n[仿真中断] 用户按下Ctrl+C，停止仿真...")
- except RuntimeError as e:
- print(f"\n[仿真错误] 运行时错误：{e}")
- traceback.print_exc()
- except Exception as e:
- print(f"\n[仿真错误] 未知异常：{e}")
- traceback.print_exc()
-
- # 资源清理
- finally:
- print("\n[资源清理] 开始清理仿真资源...")
-
- # 清理Pygame显示窗口
- if pygame_display:
- print("[资源清理] 销毁Pygame显示窗口...")
- pygame_display.destroy()
-
- # 清理绘图器
- if plotter and plotter.is_initialized:
- print("[资源清理] 清理绘图器...")
- plotter.cleanup_plot()
-
- # 销毁车辆
- if vehicle and vehicle.is_alive:
- print(f"[资源清理] 销毁车辆：{vehicle.type_id} (ID: {vehicle.id})")
- if vehicle.destroy():
- print("[资源清理] 车辆销毁成功")
- else:
- print("[资源清理] 车辆销毁失败")
-
- print("[资源清理] 仿真资源清理完成")
-
-if __name__ == "__main__":
- main()
\ No newline at end of file
From 9768bf171088cb8dc1faf8605835424a4c93e577 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 17:14:36 +0800
Subject: [PATCH 07/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E4=B8=BB?=
=?UTF-8?q?=E6=8E=A7=E5=88=B6=E4=B8=8E=E8=B7=AF=E5=BE=84=E8=B7=9F=E9=9A=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/MCP/v2.py | 247 ++++++++++++++++++---------
1 file changed, 167 insertions(+), 80 deletions(-)
diff --git a/src/autonomous_driving_car/MCP/v2.py b/src/autonomous_driving_car/MCP/v2.py
index 4e0a889bc9..174d1de37d 100644
--- a/src/autonomous_driving_car/MCP/v2.py
+++ b/src/autonomous_driving_car/MCP/v2.py
@@ -1,39 +1,37 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
-CARLA Vehicle Simulation with Visualization (sd_3/__main__.py)
+CARLA Waypoint Following with Simple Speed Control & Pygame Display (sd_4/__main__.py)
核心功能：
-1. 连接CARLA仿真服务器，清理历史车辆 Actor
-2. 生成特斯拉Model3车辆，施加恒定油门控制
-3. 基于PygameDisplay显示车辆挂载摄像头的实时画面
-4. 基于Plotter绘制车辆X坐标和速度随时间的变化曲线
-5. 设置固定的旁观者相机视角，方便观察仿真过程
+1. 连接CARLA仿真服务器，清理历史车辆Actor
+2. 生成特斯拉Model3车辆，基于预定义航点实现路径跟随
+3. 采用简易的bang-bang速度控制器，根据当前速度与目标速度调整油门/刹车
+4. 基于PygameDisplay显示车辆挂载摄像头的实时画面
+5. 基于Plotter绘制车辆X坐标、实际速度与期望速度的时间曲线
+6. 设置固定的旁观者相机视角，方便观察仿真过程
依赖：
-- tools.pygame_display.PygameDisplay：Pygame窗口显示摄像头画面
- tools.plotter_x.Plotter：Matplotlib绘制X坐标/速度曲线
+- tools.pygame_display.PygameDisplay：Pygame窗口显示摄像头画面
"""
-# 加入这几行代码，解决模块导入问题
+# ===================== 路径处理（解决tools模块导入问题）=====================
import sys
import os
-
-# 获取项目根目录（carla-python-examples）
PROJECT_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
-# 将项目根目录加入Python搜索路径
sys.path.append(PROJECT_ROOT)
+# ==============================================================================
+
import carla
import numpy as np
import time
-import pygame
-import math
import traceback
-from typing import Optional, Tuple
+from typing import Optional, List, Tuple
# 导入自定义工具类
-from tools.pygame_display import PygameDisplay
from tools.plotter_x import Plotter
+from tools.pygame_display import PygameDisplay
# ===================== 配置集中化（便于修改和维护）=====================
class SimConfig:
@@ -44,20 +42,31 @@ class SimConfig:
CARLA_TIMEOUT = 10.0 # 连接超时时间（秒）
# 车辆配置
- VEHICLE_MODEL = "vehicle.tesla.model3" # 车辆模型
- VEHICLE_ROLE_NAME = "my_car" # 车辆角色名（用于清理历史车辆）
- CONSTANT_THROTTLE = 0.8 # 恒定油门值（0-1）
- STEER_ANGLE = 0.0 # 恒定转向角（0=直线行驶）
- BRAKE_VALUE = 0.0 # 刹车值（0=不刹车）
-
- # 旁观者相机固定视角（可根据地图调整）
+ VEHICLE_MODEL = "vehicle.tesla.model3"
+ VEHICLE_ROLE_NAME = "my_car"
+
+ # 航点配置：[x坐标, y坐标, 期望速度(km/h)]
+ WAYPOINTS: List[List[float]] = [
+ [-64.0, 24.5, 40.0], # 起始点（第一段路径的参考点）
+ [40.0, 24.5, 40.0], # 制动点前的行驶点（保持40km/h）
+ [70.0, 24.5, 0.0] # 停止点（目标速度0km/h）
+ ]
+ WAYPOINT_THRESHOLD = 5.0 # 切换航点的距离阈值（米）
+ INITIAL_TARGET_WAYPOINT_ID = 1 # 初始目标航点索引（从1开始，0为起始点）
+
+ # 旁观者相机固定视角
SPECTATOR_LOCATION = carla.Location(x=63.12, y=29.88, z=5.61)
SPECTATOR_ROTATION = carla.Rotation(pitch=-4.27, yaw=-170.21, roll=0.00)
- # 绘图配置
- DESIRED_SPEED = 0.0 # 期望速度（若无速度控制，设为0）
-
# ===================== 车辆状态计算工具函数 =====================
+def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location:
+ """
+ 获取车辆当前位置
+ :param vehicle: CARLA车辆Actor对象
+ :return: 车辆的Location对象
+ """
+ return vehicle.get_location()
+
def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float:
"""
计算车辆当前速度（km/h）
@@ -65,18 +74,9 @@ def calculate_vehicle_speed_kmh(vehicle: carla.Vehicle) -> float:
:return: 车辆速度（km/h）
"""
velocity = vehicle.get_velocity()
- # 计算速度矢量的模（m/s），转换为km/h（×3.6）
speed_mps = np.linalg.norm([velocity.x, velocity.y, velocity.z])
return speed_mps * 3.6
-def get_vehicle_location(vehicle: carla.Vehicle) -> carla.Location:
- """
- 获取车辆当前位置
- :param vehicle: CARLA车辆Actor对象
- :return: 车辆的Location对象
- """
- return vehicle.get_location()
-
# ===================== 仿真资源管理工具函数 =====================
def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int:
"""
@@ -100,35 +100,98 @@ def clean_up_prev_vehicles(world: carla.World, role_name: str) -> int:
print(f"[资源清理] 共销毁{destroyed_count}辆历史车辆")
return destroyed_count
-def set_spectator_fixed_view(world: carla.World, location: carla.Location, rotation: carla.Rotation) -> bool:
+def set_spectator_fixed_view(world: carla.World) -> bool:
"""
设置旁观者相机的固定视角
:param world: CARLA的World对象
- :param location: 旁观者相机位置
- :param rotation: 旁观者相机旋转角度
:return: 是否设置成功
"""
try:
spectator = world.get_spectator()
- spectator_transform = carla.Transform(location, rotation)
+ spectator_transform = carla.Transform(
+ SimConfig.SPECTATOR_LOCATION,
+ SimConfig.SPECTATOR_ROTATION
+ )
spectator.set_transform(spectator_transform)
- print(f"\n[视角设置] 旁观者相机位置：({location.x:.2f}, {location.y:.2f}, {location.z:.2f})")
- print(f"[视角设置] 旁观者相机旋转：(俯仰：{rotation.pitch:.2f}, 偏航：{rotation.yaw:.2f}, 翻滚：{rotation.roll:.2f})")
+ print(f"\n[视角设置] 旁观者相机位置：({SimConfig.SPECTATOR_LOCATION.x:.2f}, {SimConfig.SPECTATOR_LOCATION.y:.2f}, {SimConfig.SPECTATOR_LOCATION.z:.2f})")
+ print(f"[视角设置] 旁观者相机旋转：(俯仰：{SimConfig.SPECTATOR_ROTATION.pitch:.2f}, 偏航：{SimConfig.SPECTATOR_ROTATION.yaw:.2f}, 翻滚：{SimConfig.SPECTATOR_ROTATION.roll:.2f})")
return True
except Exception as e:
print(f"[视角设置] 失败：{e}")
return False
+# ===================== 航点管理函数 =====================
+def update_target_waypoint(
+ vehicle_location: carla.Location,
+ current_target_id: int,
+ waypoints: List[List[float]],
+ threshold: float
+) -> int:
+ """
+ 检查是否到达当前目标航点，若到达则切换到下一个航点
+ :param vehicle_location: 车辆当前位置
+ :param current_target_id: 当前目标航点索引
+ :param waypoints: 航点列表
+ :param threshold: 切换航点的距离阈值
+ :return: 更新后的目标航点索引
+ """
+ # 若已是最后一个航点，不再切换
+ if current_target_id >= len(waypoints) - 1:
+ return current_target_id
+
+ # 获取当前目标航点的位置
+ target_wp_data = waypoints[current_target_id]
+ target_loc = carla.Location(x=target_wp_data[0], y=target_wp_data[1])
+
+ # 关键修改：手动计算2D距离（替代CARLA低版本不存在的distance_2d方法）
+ dx = vehicle_location.x - target_loc.x
+ dy = vehicle_location.y - target_loc.y
+ distance = np.sqrt(dx ** 2 + dy ** 2)
+
+ # 若距离小于阈值，切换到下一个航点
+ if distance < threshold:
+ print(f"[航点更新] 到达航点{current_target_id}（距离：{distance:.1f}m），新目标航点：{current_target_id + 1}")
+ return current_target_id + 1
+
+ return current_target_id
+
+# ===================== 速度控制器 =====================
+def simple_speed_controller(v_desired: float, v_current: float) -> carla.VehicleControl:
+ """
+ 简易的3状态速度控制器（bang-bang控制）
+ 根据当前速度与期望速度的差值，全量施加油门或刹车
+ :param v_desired: 期望速度（km/h）
+ :param v_current: 当前速度（km/h）
+ :return: CARLA车辆控制指令
+ """
+ control = carla.VehicleControl()
+ control.steer = 0.0 # 暂不控制转向
+ control.throttle = 0.0
+ control.brake = 0.0
+
+ # 目标速度为0时，全力刹车
+ if v_desired == 0:
+ control.brake = 1.0
+ # 当前速度小于期望速度，全力加速
+ elif v_current < v_desired:
+ control.throttle = 1.0
+ # 当前速度大于期望速度，全力刹车
+ elif v_current > v_desired:
+ control.brake = 1.0
+
+ return control
+
# ===================== 主仿真函数 =====================
def main():
"""主仿真入口函数：完成所有仿真流程的初始化、运行和清理"""
- # 初始化核心变量（所有资源对象初始化为None，便于后续清理）
+ # 初始化核心变量
client: Optional[carla.Client] = None
world: Optional[carla.World] = None
vehicle: Optional[carla.Vehicle] = None
- pygame_display: Optional[PygameDisplay] = None
plotter: Optional[Plotter] = None
+ pygame_display: Optional[PygameDisplay] = None
simulation_start_time: Optional[float] = None
+ target_waypoint_id: int = SimConfig.INITIAL_TARGET_WAYPOINT_ID
try:
# 1. 连接CARLA服务器
@@ -141,29 +204,45 @@ def main():
# 2. 清理历史车辆
clean_up_prev_vehicles(world, SimConfig.VEHICLE_ROLE_NAME)
- # 3. 获取车辆生成点
- spawn_points = world.get_map().get_spawn_points()
- if not spawn_points:
+ # 3. 生成车辆（基于第一个航点作为起始位置）
+ map_spawn_points = world.get_map().get_spawn_points()
+ if not map_spawn_points:
raise RuntimeError("[生成车辆] 地图中未找到可用的生成点！")
- spawn_point = spawn_points[0]
- print(f"[生成车辆] 选择生成点：({spawn_point.location.x:.2f}, {spawn_point.location.y:.2f}, {spawn_point.location.z:.2f})")
- # 4. 加载车辆蓝图并设置属性
+ # 获取起始航点的位置
+ start_waypoint = SimConfig.WAYPOINTS[0]
+ # 关键修改：将z轴高度从0.5提高到1.5，避免与地面碰撞
+ start_location = carla.Location(x=start_waypoint[0], y=start_waypoint[1], z=1.5)
+
+ # 找到距离起始位置最近的地图生成点（用于获取道路朝向）
+ spawn_point = min(map_spawn_points, key=lambda sp: sp.location.distance(start_location))
+ spawn_point.location = start_location # 覆盖为自定义起始位置
+
+ print(f"[生成车辆] 目标生成位置：{start_location}，使用地图朝向：{spawn_point.rotation}")
+
+ # 加载车辆蓝图
blueprint_library = world.get_blueprint_library()
vehicle_bp = blueprint_library.filter(SimConfig.VEHICLE_MODEL)[0]
vehicle_bp.set_attribute("role_name", SimConfig.VEHICLE_ROLE_NAME)
- print(f"[生成车辆] 加载车辆蓝图：{vehicle_bp.id}")
- # 5. 生成车辆
+ # 生成车辆
+ print("[生成车辆] 尝试生成车辆...")
vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+
+ # 关键修改：添加重试逻辑，第一个点失败则尝试第二个地图默认点
if vehicle is None:
- raise RuntimeError(f"[生成车辆] 在生成点{spawn_point.location}生成车辆失败！")
+ print("[生成车辆] 第一个点生成失败，尝试第二个地图默认点...")
+ spawn_point = map_spawn_points[1]
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
+ if vehicle is None:
+ raise RuntimeError(f"[生成车辆] 生成失败，请检查坐标是否合法！")
+
print(f"[生成车辆] 成功生成：{vehicle.type_id} (ID: {vehicle.id})")
- # 6. 设置旁观者相机视角
- set_spectator_fixed_view(world, SimConfig.SPECTATOR_LOCATION, SimConfig.SPECTATOR_ROTATION)
+ # 4. 设置旁观者相机视角
+ set_spectator_fixed_view(world)
- # 7. 初始化可视化组件
+ # 5. 初始化可视化组件
print("\n[可视化] 初始化绘图器（Plotter）...")
plotter = Plotter()
plotter.init_plot()
@@ -173,51 +252,59 @@ def main():
pygame_display = PygameDisplay(world, vehicle)
print("[可视化] Pygame显示窗口初始化完成")
- # 8. 记录仿真开始时间
+ # 6. 记录仿真开始时间
simulation_start_time = time.time()
- print(f"\n[仿真启动] 开始运行仿真（恒定油门：{SimConfig.CONSTANT_THROTTLE}）")
- print("[仿真启动] 按ESC或关闭Pygame窗口停止仿真...")
+ target_waypoint_id = SimConfig.INITIAL_TARGET_WAYPOINT_ID
- # 9. 主仿真循环
+ # 7. 主仿真循环
+ print("\n[仿真启动] 开始航点跟随与速度控制。按ESC或关闭Pygame窗口停止仿真...")
while True:
- # 9.1 处理Pygame事件（关闭窗口、ESC键）
+ current_loop_time = time.time()
+
+ # 处理Pygame事件（关闭窗口、ESC键）
if pygame_display.parse_events():
print("[仿真控制] 检测到Pygame退出请求，停止仿真...")
break
- # 9.2 等待仿真tick（同步仿真时间）
+ # 等待仿真tick（同步仿真时间）
world.wait_for_tick()
- # 9.3 收集车辆状态数据
- current_time = time.time()
+ # 8. 收集车辆状态数据
vehicle_location = get_vehicle_location(vehicle)
vehicle_speed = calculate_vehicle_speed_kmh(vehicle)
- sim_elapsed_time = current_time - simulation_start_time # 仿真已运行时间（秒）
-
- # 9.4 渲染Pygame窗口（摄像头画面）
- pygame_display.render()
+ sim_elapsed_time = current_loop_time - simulation_start_time
- # 9.5 更新绘图器（X坐标、速度曲线）
+ # 9. 更新可视化
+ # 更新绘图器
if plotter and plotter.is_initialized:
+ current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2]
try:
- plotter.update_plot(
- sim_elapsed_time,
- vehicle_location.x,
- vehicle_speed,
- SimConfig.DESIRED_SPEED
- )
+ plotter.update_plot(sim_elapsed_time, vehicle_location.x, vehicle_speed, current_desired_speed)
except Exception as e:
print(f"[绘图器] 更新失败（可能已关闭窗口）：{e}")
plotter.cleanup_plot()
plotter = None
- # 9.6 车辆控制：施加恒定油门
- vehicle_control = carla.VehicleControl(
- throttle=SimConfig.CONSTANT_THROTTLE,
- steer=SimConfig.STEER_ANGLE,
- brake=SimConfig.BRAKE_VALUE
+ # 渲染Pygame窗口
+ pygame_display.render()
+
+ # 10. 航点跟随逻辑
+ # 更新目标航点
+ target_waypoint_id = update_target_waypoint(
+ vehicle_location,
+ target_waypoint_id,
+ SimConfig.WAYPOINTS,
+ SimConfig.WAYPOINT_THRESHOLD
)
- vehicle.apply_control(vehicle_control)
+
+ # 获取当前目标航点的期望速度
+ current_desired_speed = SimConfig.WAYPOINTS[target_waypoint_id][2] if 0 <= target_waypoint_id < len(SimConfig.WAYPOINTS) else 0.0
+
+ # 计算车辆控制指令
+ control = simple_speed_controller(current_desired_speed, vehicle_speed)
+
+ # 应用控制指令
+ vehicle.apply_control(control)
# 异常处理
except KeyboardInterrupt:
@@ -229,7 +316,7 @@ def main():
print(f"\n[仿真错误] 未知异常：{e}")
traceback.print_exc()
- # 资源清理（无论是否异常，都执行）
+ # 资源清理
finally:
print("\n[资源清理] 开始清理仿真资源...")
From e4c440b21e54ce30f5e68dfdb0ce44c20cd1dea0 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 20:50:08 +0800
Subject: [PATCH 08/26] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=BC=AF=E9=81=93?=
=?UTF-8?q?=E8=87=AA=E9=80=82=E5=BA=94=E8=BD=AC=E5=90=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/CAS/v4.py | 338 +++++++++++++++++++++++++++
1 file changed, 338 insertions(+)
create mode 100644 src/autonomous_driving_car/CAS/v4.py
diff --git a/src/autonomous_driving_car/CAS/v4.py b/src/autonomous_driving_car/CAS/v4.py
new file mode 100644
index 0000000000..3fbfcf65c4
--- /dev/null
+++ b/src/autonomous_driving_car/CAS/v4.py
@@ -0,0 +1,338 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+CARLA 弯道自适应转向：根据弯道大小动态调整转弯幅度
+"""
+
+import sys
+import os
+import carla
+import numpy as np
+import math
+import pygame
+import traceback
+
+# ===================== 全局配置（动态参数）=====================
+# CARLA连接
+CARLA_HOST = "localhost"
+CARLA_PORT = 2000
+CARLA_TIMEOUT = 10.0
+
+# 车辆配置
+VEHICLE_MODEL = "vehicle.tesla.model3"
+VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距（米）
+VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移（正数）
+
+# 转向控制（动态参数基准）
+LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离
+LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离
+STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益
+STEER_GAIN_CURVE = 1.0 # 急弯转向增益
+STEER_DEADZONE = 0.05 # 转向死区
+STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数
+STEER_DIR_COEFF = 1.0 # 转向方向校准
+MAX_STEER = 1.0 # 最大转向角
+
+# 弯道等级划分（方向变化量阈值）
+DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值
+DIR_CHANGE_SHARP = 0.08 # 急弯阈值
+
+# 速度控制
+BASE_SPEED = 28.0 # 直道基础速度
+CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数（更明显的速度衰减）
+PID_KP = 0.15
+PID_KI = 0.01
+PID_KD = 0.01
+
+# 相机
+CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
+CAMERA_WIDTH = 800
+CAMERA_HEIGHT = 600
+CAMERA_FOV = 90
+
+# ===================== 纯追踪控制器（动态适配）=====================
+class AdaptivePurePursuit:
+ def __init__(self, wheelbase):
+ self.wheelbase = wheelbase
+ self.last_steer = 0.0 # 上一帧转向角
+ self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离
+
+ def calculate_steer(self, vehicle_transform, target_point, dir_change):
+ """
+ 自适应纯追踪计算（根据弯道大小调整参数）
+ :param vehicle_transform: 车辆变换
+ :param target_point: 目标点（carla.Location）
+ :param dir_change: 方向变化量（弯道大小）
+ :return: 自适应后的转向角
+ """
+ # 1. 获取车辆后轴位置
+ forward_vec = vehicle_transform.get_forward_vector()
+ rear_axle_loc = carla.Location(
+ x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
+ y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
+ z=vehicle_transform.location.z
+ )
+
+ # 2. 转换到车辆坐标系
+ dx = target_point.x - rear_axle_loc.x
+ dy = target_point.y - rear_axle_loc.y
+ yaw = math.radians(vehicle_transform.rotation.yaw)
+
+ dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
+ dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
+
+ # 3. 动态计算转向增益（根据弯道大小线性插值）
+ # 方向变化量越大，增益越高
+ steer_gain = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
+ )
+ steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
+
+ # 4. 纯追踪核心计算
+ if dx_vehicle < 0.1:
+ steer = self.last_steer
+ else:
+ # 纯追踪公式 + 弯道大小系数（dir_change*2 放大差异）
+ steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2)
+ steer = steer_rad / math.pi
+
+ # 应用增益和方向校准
+ steer *= steer_gain * STEER_DIR_COEFF
+
+ # 5. 应用死区
+ if abs(steer) < STEER_DEADZONE:
+ steer = 0.0
+
+ # 6. 低通滤波（平滑）
+ steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
+
+ # 7. 限制范围
+ steer = np.clip(steer, -MAX_STEER, MAX_STEER)
+
+ # 8. 更新状态
+ self.last_steer = steer
+
+ return steer
+
+ def get_adaptive_lookahead(self, dir_change):
+ """
+ 动态获取预瞄距离（根据弯道大小线性插值）
+ :param dir_change: 方向变化量
+ :return: 自适应预瞄距离
+ """
+ lookahead_dist = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
+ )
+ lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
+ self.last_lookahead = lookahead_dist
+ return lookahead_dist
+
+# ===================== 速度控制器（动态调整）=====================
+class SpeedController:
+ def __init__(self, kp, ki, kd):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.last_error = 0.0
+ self.integral = 0.0
+
+ def calculate(self, target_speed, current_speed):
+ """PID速度控制"""
+ error = target_speed - current_speed
+
+ p = self.kp * error
+ self.integral += self.ki * error
+ self.integral = np.clip(self.integral, -1.0, 1.0)
+ i = self.integral
+ d = self.kd * (error - self.last_error)
+ self.last_error = error
+
+ output = p + i + d
+ return np.clip(output, 0.0, 1.0)
+
+# ===================== 辅助函数：计算方向变化（替代曲率）=====================
+def calculate_dir_change(current_wp):
+ """
+ 计算相邻Waypoint的方向变化量（更精细的计算，增加采样点）
+ :param current_wp: 当前Waypoint
+ :return: 方向变化的绝对值之和，弯道等级（0=直道，1=缓弯，2=急弯）
+ """
+ waypoints = [current_wp]
+ # 增加采样点到5个，更准确的判断弯道大小
+ for i in range(5):
+ next_wps = waypoints[-1].next(1.0)
+ if next_wps:
+ waypoints.append(next_wps[0])
+ else:
+ break
+
+ if len(waypoints) < 4:
+ return 0.0, 0
+
+ # 计算每个相邻点的方向
+ dirs = []
+ for i in range(1, len(waypoints)):
+ wp_prev = waypoints[i-1]
+ wp_curr = waypoints[i]
+ dir_rad = math.atan2(
+ wp_curr.transform.location.y - wp_prev.transform.location.y,
+ wp_curr.transform.location.x - wp_prev.transform.location.x
+ )
+ dirs.append(dir_rad)
+
+ # 计算方向变化的绝对值之和（放大差异）
+ dir_change = 0.0
+ for i in range(1, len(dirs)):
+ dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异
+
+ # 划分弯道等级
+ if dir_change < DIR_CHANGE_GENTLE:
+ curve_level = 0 # 直道
+ elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 1 # 缓弯
+ else:
+ curve_level = 2 # 急弯
+
+ return dir_change, curve_level
+
+# ===================== 相机管理器 =====================
+class CameraManager:
+ def __init__(self, world, vehicle, display):
+ self.world = world
+ self.vehicle = vehicle
+ self.display = display
+ self.camera = None
+ self._create_camera()
+
+ def _create_camera(self):
+ bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
+ bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
+ bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
+ bp.set_attribute("fov", str(CAMERA_FOV))
+ self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
+ self.camera.listen(self._on_image)
+
+ def _on_image(self, image):
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3]
+ array = array[:, :, ::-1].swapaxes(0, 1)
+ self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
+ pygame.display.flip()
+
+ def destroy(self):
+ if self.camera:
+ self.camera.stop()
+ self.camera.destroy()
+
+# ===================== 主函数（核心逻辑）=====================
+def main():
+ pygame.init()
+ display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
+ pygame.display.set_caption("CARLA 弯道自适应转向（最终版）")
+
+ client = None
+ world = None
+ vehicle = None
+ camera_manager = None
+ pp_controller = None
+ speed_controller = None
+
+ try:
+ # 1. 连接CARLA并初始化
+ client = carla.Client(CARLA_HOST, CARLA_PORT)
+ client.set_timeout(CARLA_TIMEOUT)
+ world = client.get_world()
+ map = world.get_map()
+
+ # 清理现有车辆
+ for actor in world.get_actors().filter("vehicle.*"):
+ actor.destroy()
+
+ # 生成车辆
+ vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
+ spawn_points = map.get_spawn_points()
+ vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])
+ print(f"车辆生成成功：{vehicle.type_id}")
+
+ # 初始化控制器
+ pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
+ camera_manager = CameraManager(world, vehicle, display)
+
+ # 2. 主循环
+ print("仿真启动，按ESC退出...")
+ clock = pygame.time.Clock()
+ running = True
+
+ while running:
+ # 事件处理
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
+ running = False
+
+ # 3. 获取关键数据
+ vehicle_transform = vehicle.get_transform()
+ vehicle_vel = vehicle.get_velocity()
+ current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h
+
+ # 获取当前Waypoint
+ current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True)
+
+ # 计算方向变化量和弯道等级
+ dir_change, curve_level = calculate_dir_change(current_wp)
+
+ # 动态获取预瞄距离
+ lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)
+
+ # 获取目标Waypoint（动态预瞄距离）
+ target_wps = current_wp.next(lookahead_dist)
+ target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+
+ # 4. 计算目标速度（根据弯道等级调整）
+ curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数
+ speed_factor = curve_speed_factors[min(curve_level, 2)]
+ target_speed = BASE_SPEED * speed_factor
+ target_speed = max(10.0, target_speed)
+
+ # 5. 计算转向角（传入方向变化量，实现自适应）
+ steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
+
+ # 6. 计算油门/刹车
+ throttle = speed_controller.calculate(target_speed, current_speed)
+ brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0
+
+ # 7. 应用控制
+ control = carla.VehicleControl()
+ control.steer = steer
+ control.throttle = throttle
+ control.brake = brake
+ control.hand_brake = False
+ vehicle.apply_control(control)
+
+ # 8. 打印状态（调试用，显示弯道等级和动态参数）
+ curve_names = ["直道", "缓弯", "急弯"]
+ print(f"速度：{current_speed:.1f}km/h | 目标速度：{target_speed:.1f}km/h | 弯道：{curve_names[curve_level]} | 预瞄：{lookahead_dist:.1f}m | 转向：{steer:.3f}")
+
+ # 控制帧率
+ clock.tick(30)
+
+ except Exception as e:
+ print(f"错误：{e}")
+ traceback.print_exc()
+
+ finally:
+ # 清理资源
+ print("清理资源...")
+ if camera_manager:
+ camera_manager.destroy()
+ if vehicle:
+ vehicle.destroy()
+ pygame.quit()
+ print("仿真结束")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From 85e2c10d83d883b0c7e55c1537e46b65fd46d020 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 21:14:28 +0800
Subject: [PATCH 09/26] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=BC=AF=E9=81=93?=
=?UTF-8?q?=E8=87=AA=E9=80=82=E5=BA=94=E8=BD=AC=E5=90=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 668 ++++++++++++++---------------
1 file changed, 319 insertions(+), 349 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 10a4076619..3fbfcf65c4 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,368 +1,338 @@
#!/usr/bin/env python
+# -*- coding: utf-8 -*-
"""
-CARLA Basic Vehicle Spawn and Spectator Setup (sd_1/__main__.py)
-完全适配 CARLA 0.9.15 版本（无任何天气预设依赖）
-
-This script connects to a CARLA simulator instance, removes any
-pre-existing vehicles with the role 'my_car', spawns a new
-Tesla Model 3 at a default spawn point, and positions the
-spectator camera behind the newly spawned vehicle.
-新增功能：定时循环切换CARLA模拟器的天气（晴天、多云、雨天、雾天、日落等）
-
-The script keeps the simulation running until interrupted (Ctrl+C),
-but the vehicle does not move.
+CARLA 弯道自适应转向：根据弯道大小动态调整转弯幅度
"""
-# 导入CARLA模拟器的Python API
+import sys
+import os
import carla
-# 导入时间模块，用于延时和计时
-import time
-
-
-def remove_previous_vehicle(world: carla.World) -> None:
- """
- 查找并销毁所有角色名为'my_car'的车辆Actor，避免重复生成导致冲突
-
- Args:
- world (carla.World): CARLA模拟器的世界对象，用于获取当前所有Actor
- """
- print("Searching for previous 'my_car' vehicles...")
- # 过滤出所有车辆类型的Actor（vehicle.* 匹配所有车辆蓝图）
- actors = world.get_actors().filter('vehicle.*')
- # 记录成功销毁的车辆数量
- removed_count = 0
-
- for actor in actors:
- # 检查Actor的角色名是否为'my_car'
- if actor.attributes.get('role_name') == 'my_car':
- print(f" - Removing previous vehicle: {actor.type_id} (ID {actor.id})")
- # 销毁Actor，返回布尔值表示是否成功
- if actor.destroy():
- removed_count += 1
- else:
- print(f" - Failed to remove vehicle {actor.id}")
-
- print(f"Removed {removed_count} previous vehicles.")
-
-
-def set_spectator_behind_vehicle(world: carla.World, vehicle: carla.Vehicle) -> None:
- """
- 将旁观者相机（spectator）定位到指定车辆的后上方，实现跟随视角
-
- Args:
- world (carla.World): CARLA模拟器的世界对象
- vehicle (carla.Vehicle): 目标车辆Actor，用于获取车辆的位置和姿态
- """
- # 获取旁观者相机Actor（CARLA中全局唯一的 spectator）
- spectator = world.get_spectator()
- # 获取车辆的当前位姿（位置+旋转）
- vehicle_transform = vehicle.get_transform()
- # 获取车辆的前向向量，用于计算相机的相对偏移（保证相机始终在车辆后方）
- forward_vector = vehicle_transform.get_forward_vector()
-
- # 计算相机偏移：向后15米，向上6米（基于车辆的前向向量，保证方向正确）
- camera_offset = carla.Location(
- x=-15 * forward_vector.x,
- y=-15 * forward_vector.y,
- z=6
- )
- # 构建旁观者相机的位姿：
- # 位置 = 车辆位置 + 偏移量
- # 旋转 = 俯仰角-20°（向下看），偏航角与车辆一致，滚转角为0
- spectator_transform = carla.Transform(
- vehicle_transform.location + camera_offset,
- carla.Rotation(
- pitch=-20, # 俯仰角，负数表示向下看
- yaw=vehicle_transform.rotation.yaw, # 偏航角与车辆一致
- roll=0 # 滚转角，保持水平
+import numpy as np
+import math
+import pygame
+import traceback
+
+# ===================== 全局配置（动态参数）=====================
+# CARLA连接
+CARLA_HOST = "localhost"
+CARLA_PORT = 2000
+CARLA_TIMEOUT = 10.0
+
+# 车辆配置
+VEHICLE_MODEL = "vehicle.tesla.model3"
+VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距（米）
+VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移（正数）
+
+# 转向控制（动态参数基准）
+LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离
+LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离
+STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益
+STEER_GAIN_CURVE = 1.0 # 急弯转向增益
+STEER_DEADZONE = 0.05 # 转向死区
+STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数
+STEER_DIR_COEFF = 1.0 # 转向方向校准
+MAX_STEER = 1.0 # 最大转向角
+
+# 弯道等级划分（方向变化量阈值）
+DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值
+DIR_CHANGE_SHARP = 0.08 # 急弯阈值
+
+# 速度控制
+BASE_SPEED = 28.0 # 直道基础速度
+CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数（更明显的速度衰减）
+PID_KP = 0.15
+PID_KI = 0.01
+PID_KD = 0.01
+
+# 相机
+CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
+CAMERA_WIDTH = 800
+CAMERA_HEIGHT = 600
+CAMERA_FOV = 90
+
+# ===================== 纯追踪控制器（动态适配）=====================
+class AdaptivePurePursuit:
+ def __init__(self, wheelbase):
+ self.wheelbase = wheelbase
+ self.last_steer = 0.0 # 上一帧转向角
+ self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离
+
+ def calculate_steer(self, vehicle_transform, target_point, dir_change):
+ """
+ 自适应纯追踪计算（根据弯道大小调整参数）
+ :param vehicle_transform: 车辆变换
+ :param target_point: 目标点（carla.Location）
+ :param dir_change: 方向变化量（弯道大小）
+ :return: 自适应后的转向角
+ """
+ # 1. 获取车辆后轴位置
+ forward_vec = vehicle_transform.get_forward_vector()
+ rear_axle_loc = carla.Location(
+ x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
+ y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
+ z=vehicle_transform.location.z
)
- )
-
- # 尝试设置相机位姿，增加异常处理提高鲁棒性
- try:
- spectator.set_transform(spectator_transform)
- print("Spectator camera positioned behind the vehicle.")
- except Exception as e:
- print(f"Error setting spectator transform: {e}")
+ # 2. 转换到车辆坐标系
+ dx = target_point.x - rear_axle_loc.x
+ dy = target_point.y - rear_axle_loc.y
+ yaw = math.radians(vehicle_transform.rotation.yaw)
-def get_weather_presets() -> list:
- """
- 纯手动定义天气参数列表（完全不依赖CARLA预设，适配0.9.15）
- 每个天气通过手动设置WeatherParameters的所有关键参数实现，确保兼容性
-
- Returns:
- list: 元组列表，每个元组包含（天气名称，carla.WeatherParameters对象）
- """
- # 1. 晴天中午（太阳高悬、无云、无雨、无雾）
- clear_noon = carla.WeatherParameters(
- sun_altitude_angle=75.0, # 太阳高度角（75°=中午，天顶为90°）
- sun_azimuth_angle=90.0, # 太阳方位角
- cloudiness=0.0, # 云量（0=无云）
- precipitation=0.0, # 降水量（0=无雨）
- precipitation_deposits=0.0, # 降水沉积（路面雨水）
- wind_intensity=5.0, # 风力
- fog_density=0.0, # 雾密度（0=无雾）
- fog_distance=0.0, # 雾的可见距离
- fog_falloff=1.0, # 雾的衰减率
- wetness=0.0, # 路面湿度
- scattering_intensity=0.0, # 光的散射强度
- mie_scattering_scale=0.0, # 米氏散射比例
- rayleigh_scattering_scale=0.0 # 瑞利散射比例
- )
-
- # 2. 多云中午（高云量，阳光散射）
- cloudy_noon = carla.WeatherParameters(
- sun_altitude_angle=75.0,
- sun_azimuth_angle=90.0,
- cloudiness=80.0, # 云量80%
- precipitation=0.0,
- precipitation_deposits=0.0,
- wind_intensity=10.0,
- fog_density=0.0,
- fog_distance=0.0,
- fog_falloff=1.0,
- wetness=0.0,
- scattering_intensity=0.1,
- mie_scattering_scale=0.1,
- rayleigh_scattering_scale=0.1
- )
-
- # 3. 小雨中午（少量降雨、路面微湿）
- light_rain_noon = carla.WeatherParameters(
- sun_altitude_angle=75.0,
- sun_azimuth_angle=90.0,
- cloudiness=90.0, # 云量90%
- precipitation=20.0, # 降水量20%（小雨）
- precipitation_deposits=5.0, # 路面雨水沉积5%
- wind_intensity=15.0,
- fog_density=5.0, # 轻微雾霭
- fog_distance=50.0,
- fog_falloff=0.8,
- wetness=0.2, # 路面湿度20%
- scattering_intensity=0.2,
- mie_scattering_scale=0.2,
- rayleigh_scattering_scale=0.2
- )
-
- # 4. 中雨中午（中等降雨、路面湿滑）
- mid_rain_noon = carla.WeatherParameters(
- sun_altitude_angle=75.0,
- sun_azimuth_angle=90.0,
- cloudiness=100.0, # 满云
- precipitation=50.0, # 降水量50%（中雨）
- precipitation_deposits=20.0, # 路面雨水沉积20%
- wind_intensity=20.0,
- fog_density=15.0, # 雾密度15%
- fog_distance=30.0,
- fog_falloff=0.6,
- wetness=0.5, # 路面湿度50%
- scattering_intensity=0.3,
- mie_scattering_scale=0.3,
- rayleigh_scattering_scale=0.3
- )
-
- # 5. 雾天中午（大雾、能见度低）
- mist_noon = carla.WeatherParameters(
- sun_altitude_angle=75.0,
- sun_azimuth_angle=90.0,
- cloudiness=50.0,
- precipitation=0.0,
- precipitation_deposits=0.0,
- wind_intensity=5.0,
- fog_density=30.0, # 雾密度30%（大雾）
- fog_distance=10.0, # 雾的可见距离10米
- fog_falloff=0.5,
- wetness=0.1,
- scattering_intensity=0.4,
- mie_scattering_scale=0.4,
- rayleigh_scattering_scale=0.4
- )
-
- # 6. 晴天日落（太阳低垂、暖色调、无云）
- clear_sunset = carla.WeatherParameters(
- sun_altitude_angle=15.0, # 太阳高度角15°（日落，地平线为0°）
- sun_azimuth_angle=180.0, # 太阳方位角180°（西方）
- cloudiness=0.0,
- precipitation=0.0,
- precipitation_deposits=0.0,
- wind_intensity=5.0,
- fog_density=0.0,
- fog_distance=0.0,
- fog_falloff=1.0,
- wetness=0.0,
- scattering_intensity=0.1,
- mie_scattering_scale=0.1,
- rayleigh_scattering_scale=0.1
- )
-
- # 7. 潮湿路面（无雨但路面湿滑、轻微雾）
- wet_road = carla.WeatherParameters(
- sun_altitude_angle=75.0,
- sun_azimuth_angle=90.0,
- cloudiness=30.0,
- precipitation=0.0,
- precipitation_deposits=0.0,
- wind_intensity=10.0,
- fog_density=5.0,
- fog_distance=40.0,
- fog_falloff=0.9,
- wetness=0.8, # 路面湿度80%（湿滑）
- scattering_intensity=0.1,
- mie_scattering_scale=0.1,
- rayleigh_scattering_scale=0.1
- )
-
- # 组合天气预设列表
- weather_presets = [
- ("Clear Noon", clear_noon),
- ("Cloudy Noon", cloudy_noon),
- ("Light Rain Noon", light_rain_noon),
- ("Mid Rain Noon", mid_rain_noon),
- ("Mist Noon", mist_noon),
- ("Clear Sunset", clear_sunset),
- ("Wet Road Noon", wet_road)
- ]
-
- return weather_presets
-
-
-def switch_weather(world: carla.World, weather: carla.WeatherParameters, weather_name: str) -> None:
- """
- 设置CARLA世界的天气，并打印切换信息
-
- Args:
- world (carla.World): CARLA模拟器的世界对象
- weather (carla.WeatherParameters): 目标天气参数对象
- weather_name (str): 天气名称，用于打印日志
- """
- try:
- # 设置世界天气
- world.set_weather(weather)
- print(f"\n=== Switched to weather: {weather_name} ===")
- except Exception as e:
- print(f"Error switching weather to {weather_name}: {e}")
+ dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
+ dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
+ # 3. 动态计算转向增益（根据弯道大小线性插值）
+ # 方向变化量越大，增益越高
+ steer_gain = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
+ )
+ steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
-def main() -> None:
+ # 4. 纯追踪核心计算
+ if dx_vehicle < 0.1:
+ steer = self.last_steer
+ else:
+ # 纯追踪公式 + 弯道大小系数（dir_change*2 放大差异）
+ steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2)
+ steer = steer_rad / math.pi
+
+ # 应用增益和方向校准
+ steer *= steer_gain * STEER_DIR_COEFF
+
+ # 5. 应用死区
+ if abs(steer) < STEER_DEADZONE:
+ steer = 0.0
+
+ # 6. 低通滤波（平滑）
+ steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
+
+ # 7. 限制范围
+ steer = np.clip(steer, -MAX_STEER, MAX_STEER)
+
+ # 8. 更新状态
+ self.last_steer = steer
+
+ return steer
+
+ def get_adaptive_lookahead(self, dir_change):
+ """
+ 动态获取预瞄距离（根据弯道大小线性插值）
+ :param dir_change: 方向变化量
+ :return: 自适应预瞄距离
+ """
+ lookahead_dist = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
+ )
+ lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
+ self.last_lookahead = lookahead_dist
+ return lookahead_dist
+
+# ===================== 速度控制器（动态调整）=====================
+class SpeedController:
+ def __init__(self, kp, ki, kd):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.last_error = 0.0
+ self.integral = 0.0
+
+ def calculate(self, target_speed, current_speed):
+ """PID速度控制"""
+ error = target_speed - current_speed
+
+ p = self.kp * error
+ self.integral += self.ki * error
+ self.integral = np.clip(self.integral, -1.0, 1.0)
+ i = self.integral
+ d = self.kd * (error - self.last_error)
+ self.last_error = error
+
+ output = p + i + d
+ return np.clip(output, 0.0, 1.0)
+
+# ===================== 辅助函数：计算方向变化（替代曲率）=====================
+def calculate_dir_change(current_wp):
"""
- 主执行函数：
- 1. 连接CARLA服务器
- 2. 清理旧车辆
- 3. 生成特斯拉Model3车辆
- 4. 设置旁观者相机
- 5. 定时循环切换天气
- 6. 保持仿真运行直到用户中断
+ 计算相邻Waypoint的方向变化量（更精细的计算，增加采样点）
+ :param current_wp: 当前Waypoint
+ :return: 方向变化的绝对值之和，弯道等级（0=直道，1=缓弯，2=急弯）
"""
- # 初始化变量，避免finally块中引用未定义的变量
- client: carla.Client = None
- world: carla.World = None
- vehicle: carla.Vehicle = None
-
- # 天气相关变量初始化
- weather_presets = get_weather_presets() # 获取天气预设列表
- current_weather_index = 0 # 当前天气的索引
- weather_switch_interval = 10 # 天气切换间隔（秒）
- last_weather_switch_time = time.time() # 上一次天气切换的时间戳
+ waypoints = [current_wp]
+ # 增加采样点到5个，更准确的判断弯道大小
+ for i in range(5):
+ next_wps = waypoints[-1].next(1.0)
+ if next_wps:
+ waypoints.append(next_wps[0])
+ else:
+ break
+
+ if len(waypoints) < 4:
+ return 0.0, 0
+
+ # 计算每个相邻点的方向
+ dirs = []
+ for i in range(1, len(waypoints)):
+ wp_prev = waypoints[i-1]
+ wp_curr = waypoints[i]
+ dir_rad = math.atan2(
+ wp_curr.transform.location.y - wp_prev.transform.location.y,
+ wp_curr.transform.location.x - wp_prev.transform.location.x
+ )
+ dirs.append(dir_rad)
+
+ # 计算方向变化的绝对值之和（放大差异）
+ dir_change = 0.0
+ for i in range(1, len(dirs)):
+ dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异
+
+ # 划分弯道等级
+ if dir_change < DIR_CHANGE_GENTLE:
+ curve_level = 0 # 直道
+ elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 1 # 缓弯
+ else:
+ curve_level = 2 # 急弯
+
+ return dir_change, curve_level
+
+# ===================== 相机管理器 =====================
+class CameraManager:
+ def __init__(self, world, vehicle, display):
+ self.world = world
+ self.vehicle = vehicle
+ self.display = display
+ self.camera = None
+ self._create_camera()
+
+ def _create_camera(self):
+ bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
+ bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
+ bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
+ bp.set_attribute("fov", str(CAMERA_FOV))
+ self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
+ self.camera.listen(self._on_image)
+
+ def _on_image(self, image):
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3]
+ array = array[:, :, ::-1].swapaxes(0, 1)
+ self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
+ pygame.display.flip()
+
+ def destroy(self):
+ if self.camera:
+ self.camera.stop()
+ self.camera.destroy()
+
+# ===================== 主函数（核心逻辑）=====================
+def main():
+ pygame.init()
+ display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
+ pygame.display.set_caption("CARLA 弯道自适应转向（最终版）")
+
+ client = None
+ world = None
+ vehicle = None
+ camera_manager = None
+ pp_controller = None
+ speed_controller = None
try:
- # 连接到本地CARLA服务器（地址：localhost，端口：2000）
- client = carla.Client('localhost', 2000)
- # 设置连接超时时间（10秒），避免无限等待
- client.set_timeout(10.0)
- print("Connecting to CARLA server...")
-
- # 获取当前CARLA世界对象（包含地图、Actor、天气等信息）
+ # 1. 连接CARLA并初始化
+ client = carla.Client(CARLA_HOST, CARLA_PORT)
+ client.set_timeout(CARLA_TIMEOUT)
world = client.get_world()
- # 打印当前加载的地图名称
- print(f"Connected to world: {world.get_map().name}")
-
- # 清理之前运行残留的'my_car'车辆
- remove_previous_vehicle(world)
-
- # 获取地图的所有预设生成点（用于车辆/行人的生成）
- spawn_points = world.get_map().get_spawn_points()
- if not spawn_points:
- print("Error: No spawn points found on the map!")
- return
- # 选择第一个生成点作为车辆生成位置
- spawn_point = spawn_points[0]
-
- # 获取蓝图库（包含所有可生成的Actor蓝图：车辆、行人、传感器等）
- vehicle_bp_library = world.get_blueprint_library()
- # 过滤出特斯拉Model3的蓝图（vehicle.tesla.model3 是CARLA中该车辆的唯一标识）
- vehicle_bp = vehicle_bp_library.filter('vehicle.tesla.model3')[0]
- # 设置车辆的角色名，方便后续清理
- vehicle_bp.set_attribute('role_name', 'my_car')
-
- # 尝试生成车辆（try_spawn_actor会检查生成点是否被占用，返回None表示失败）
- print("Attempting to spawn vehicle...")
- vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
-
- if vehicle is None:
- # 生成失败（可能生成点被占用）
- print(f"Error: Failed to spawn vehicle at {spawn_point.location}.")
- return
-
- # 打印生成成功的车辆信息
- print(f"Vehicle {vehicle.type_id} (ID {vehicle.id}) spawned successfully.")
-
- # 等待车辆稳定（等待一次仿真tick，再加0.5秒延时）
- world.wait_for_tick()
- time.sleep(0.5)
-
- # 设置旁观者相机到车辆后上方
- set_spectator_behind_vehicle(world, vehicle)
-
- # 初始化天气为第一个预设
- switch_weather(world, weather_presets[0][1], weather_presets[0][0])
-
- # 打印运行提示
- print(f"\nSimulation running. Vehicle is stationary.")
- print(f"Weather will switch every {weather_switch_interval} seconds.")
- print("Press Ctrl+C to stop.\n")
-
- # 主循环：保持仿真运行并定时切换天气
- while True:
- # 等待仿真tick（推进仿真时间）
- world.wait_for_tick()
-
- # 检查是否到达天气切换时间
- current_time = time.time()
- if current_time - last_weather_switch_time >= weather_switch_interval:
- # 切换到下一个天气（循环遍历预设列表）
- current_weather_index = (current_weather_index + 1) % len(weather_presets)
- weather_name, weather = weather_presets[current_weather_index]
- switch_weather(world, weather, weather_name)
- # 更新上一次切换时间
- last_weather_switch_time = current_time
-
- # 小延时，避免循环过于频繁（减少CPU占用）
- time.sleep(0.1)
-
- except KeyboardInterrupt:
- # 捕获用户Ctrl+C中断，友好退出
- print("\nScript stopped by user (Ctrl+C).")
+ map = world.get_map()
+
+ # 清理现有车辆
+ for actor in world.get_actors().filter("vehicle.*"):
+ actor.destroy()
+
+ # 生成车辆
+ vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
+ spawn_points = map.get_spawn_points()
+ vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])
+ print(f"车辆生成成功：{vehicle.type_id}")
+
+ # 初始化控制器
+ pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
+ camera_manager = CameraManager(world, vehicle, display)
+
+ # 2. 主循环
+ print("仿真启动，按ESC退出...")
+ clock = pygame.time.Clock()
+ running = True
+
+ while running:
+ # 事件处理
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
+ running = False
+
+ # 3. 获取关键数据
+ vehicle_transform = vehicle.get_transform()
+ vehicle_vel = vehicle.get_velocity()
+ current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h
+
+ # 获取当前Waypoint
+ current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True)
+
+ # 计算方向变化量和弯道等级
+ dir_change, curve_level = calculate_dir_change(current_wp)
+
+ # 动态获取预瞄距离
+ lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)
+
+ # 获取目标Waypoint（动态预瞄距离）
+ target_wps = current_wp.next(lookahead_dist)
+ target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+
+ # 4. 计算目标速度（根据弯道等级调整）
+ curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数
+ speed_factor = curve_speed_factors[min(curve_level, 2)]
+ target_speed = BASE_SPEED * speed_factor
+ target_speed = max(10.0, target_speed)
+
+ # 5. 计算转向角（传入方向变化量，实现自适应）
+ steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
+
+ # 6. 计算油门/刹车
+ throttle = speed_controller.calculate(target_speed, current_speed)
+ brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0
+
+ # 7. 应用控制
+ control = carla.VehicleControl()
+ control.steer = steer
+ control.throttle = throttle
+ control.brake = brake
+ control.hand_brake = False
+ vehicle.apply_control(control)
+
+ # 8. 打印状态（调试用，显示弯道等级和动态参数）
+ curve_names = ["直道", "缓弯", "急弯"]
+ print(f"速度：{current_speed:.1f}km/h | 目标速度：{target_speed:.1f}km/h | 弯道：{curve_names[curve_level]} | 预瞄：{lookahead_dist:.1f}m | 转向：{steer:.3f}")
+
+ # 控制帧率
+ clock.tick(30)
+
except Exception as e:
- # 捕获其他未预期的异常，打印错误信息和堆栈跟踪
- print(f"\nAn unexpected error occurred: {e}")
- import traceback
+ print(f"错误：{e}")
traceback.print_exc()
- finally:
- # 资源清理：确保车辆被销毁，避免残留
- print("\nStarting resource cleanup...")
- if vehicle is not None and vehicle.is_alive:
- print(f"Destroying vehicle: {vehicle.type_id} (ID {vehicle.id})")
- if vehicle.destroy():
- print("Vehicle destroyed successfully.")
- else:
- print("Vehicle destroy() returned False.")
- else:
- print("Vehicle was None or not alive, no destruction needed.")
-
- print("Simulation finished.")
-
-# 程序入口
-if __name__ == '__main__':
+ finally:
+ # 清理资源
+ print("清理资源...")
+ if camera_manager:
+ camera_manager.destroy()
+ if vehicle:
+ vehicle.destroy()
+ pygame.quit()
+ print("仿真结束")
+
+if __name__ == "__main__":
main()
\ No newline at end of file
From 51bb9b3033b91787ac063d8885b1568d25cd0096 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Fri, 19 Dec 2025 22:02:15 +0800
Subject: [PATCH 10/26] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E8=BD=A6=E8=BE=86?=
=?UTF-8?q?=E6=B2=BF=E9=81=93=E8=B7=AF=E7=9A=84=E8=87=AA=E4=B8=BB=E8=88=AA?=
=?UTF-8?q?=E7=82=B9=E8=B7=9F=E8=B8=AA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 855 +++++++++++++++++++----------
1 file changed, 552 insertions(+), 303 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 3fbfcf65c4..64f88ed002 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,338 +1,587 @@
#!/usr/bin/env python
-# -*- coding: utf-8 -*-
"""
-CARLA 弯道自适应转向：根据弯道大小动态调整转弯幅度
+CARLA Waypoint Following - Dynamic Road Waypoints (sd_6/__main__.py)
+
+核心改进：
+1. 动态生成沿道路的航点，替代固定坐标，避开障碍物
+2. 选择地图开阔区域生成车辆（Town03主干道）
+3. 增加障碍物检测，确保行驶路段安全
+4. 优化航点跟踪逻辑，适配动态道路
"""
+# ===================== 系统导入 & 路径配置 =====================
import sys
import os
import carla
import numpy as np
+import time
import math
+import logging
+from typing import List, Tuple, Optional, Union
import pygame
-import traceback
-
-# ===================== 全局配置（动态参数）=====================
-# CARLA连接
-CARLA_HOST = "localhost"
-CARLA_PORT = 2000
-CARLA_TIMEOUT = 10.0
-
-# 车辆配置
-VEHICLE_MODEL = "vehicle.tesla.model3"
-VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距（米）
-VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移（正数）
-
-# 转向控制（动态参数基准）
-LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离
-LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离
-STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益
-STEER_GAIN_CURVE = 1.0 # 急弯转向增益
-STEER_DEADZONE = 0.05 # 转向死区
-STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数
-STEER_DIR_COEFF = 1.0 # 转向方向校准
-MAX_STEER = 1.0 # 最大转向角
-
-# 弯道等级划分（方向变化量阈值）
-DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值
-DIR_CHANGE_SHARP = 0.08 # 急弯阈值
-
-# 速度控制
-BASE_SPEED = 28.0 # 直道基础速度
-CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数（更明显的速度衰减）
-PID_KP = 0.15
-PID_KI = 0.01
-PID_KD = 0.01
-
-# 相机
-CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
-CAMERA_WIDTH = 800
-CAMERA_HEIGHT = 600
-CAMERA_FOV = 90
-
-# ===================== 纯追踪控制器（动态适配）=====================
-class AdaptivePurePursuit:
- def __init__(self, wheelbase):
- self.wheelbase = wheelbase
- self.last_steer = 0.0 # 上一帧转向角
- self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离
-
- def calculate_steer(self, vehicle_transform, target_point, dir_change):
- """
- 自适应纯追踪计算（根据弯道大小调整参数）
- :param vehicle_transform: 车辆变换
- :param target_point: 目标点（carla.Location）
- :param dir_change: 方向变化量（弯道大小）
- :return: 自适应后的转向角
- """
- # 1. 获取车辆后轴位置
- forward_vec = vehicle_transform.get_forward_vector()
- rear_axle_loc = carla.Location(
- x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
- y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
- z=vehicle_transform.location.z
- )
-
- # 2. 转换到车辆坐标系
- dx = target_point.x - rear_axle_loc.x
- dy = target_point.y - rear_axle_loc.y
- yaw = math.radians(vehicle_transform.rotation.yaw)
-
- dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
- dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
-
- # 3. 动态计算转向增益（根据弯道大小线性插值）
- # 方向变化量越大，增益越高
- steer_gain = np.interp(
- dir_change,
- [0, DIR_CHANGE_SHARP],
- [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
- )
- steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
-
- # 4. 纯追踪核心计算
- if dx_vehicle < 0.1:
- steer = self.last_steer
- else:
- # 纯追踪公式 + 弯道大小系数（dir_change*2 放大差异）
- steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2)
- steer = steer_rad / math.pi
-
- # 应用增益和方向校准
- steer *= steer_gain * STEER_DIR_COEFF
-
- # 5. 应用死区
- if abs(steer) < STEER_DEADZONE:
- steer = 0.0
-
- # 6. 低通滤波（平滑）
- steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
-
- # 7. 限制范围
- steer = np.clip(steer, -MAX_STEER, MAX_STEER)
+import matplotlib.pyplot as plt
+
+# 获取当前脚本目录并加入搜索路径
+current_dir = os.path.dirname(os.path.abspath(__file__))
+sys.path.append(current_dir)
+
+# ===================== PID控制器（优化版）=====================
+class PIDController:
+ """PID速度控制器"""
+ def __init__(self):
+ self.kp = 0.3
+ self.ki = 0.01
+ self.kd = 0.1
+ self.prev_error = 0.0
+ self.integral = 0.0
+ self.integral_limit = 0.5
- # 8. 更新状态
- self.last_steer = steer
+ def calculate_control(self, target_speed, current_speed):
+ error = target_speed - current_speed
+ self.integral = np.clip(self.integral + error * self.ki, -self.integral_limit, self.integral_limit)
+ derivative = (error - self.prev_error) * self.kd
+ output = self.kp * error + self.integral + derivative
- return steer
+ throttle = max(0.0, min(1.0, output)) if output > 0 else 0.0
+ brake = max(0.0, min(1.0, -output)) if output < 0 else 0.0
- def get_adaptive_lookahead(self, dir_change):
- """
- 动态获取预瞄距离（根据弯道大小线性插值）
- :param dir_change: 方向变化量
- :return: 自适应预瞄距离
- """
- lookahead_dist = np.interp(
- dir_change,
- [0, DIR_CHANGE_SHARP],
- [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
- )
- lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
- self.last_lookahead = lookahead_dist
- return lookahead_dist
-
-# ===================== 速度控制器（动态调整）=====================
-class SpeedController:
- def __init__(self, kp, ki, kd):
- self.kp = kp
- self.ki = ki
- self.kd = kd
- self.last_error = 0.0
- self.integral = 0.0
+ self.prev_error = error
- def calculate(self, target_speed, current_speed):
- """PID速度控制"""
- error = target_speed - current_speed
+ from collections import namedtuple
+ Control = namedtuple('Control', ['throttle', 'brake'])
+ return Control(throttle=throttle, brake=brake)
- p = self.kp * error
- self.integral += self.ki * error
- self.integral = np.clip(self.integral, -1.0, 1.0)
- i = self.integral
- d = self.kd * (error - self.last_error)
- self.last_error = error
-
- output = p + i + d
- return np.clip(output, 0.0, 1.0)
-
-# ===================== 辅助函数：计算方向变化（替代曲率）=====================
-def calculate_dir_change(current_wp):
- """
- 计算相邻Waypoint的方向变化量（更精细的计算，增加采样点）
- :param current_wp: 当前Waypoint
- :return: 方向变化的绝对值之和，弯道等级（0=直道，1=缓弯，2=急弯）
- """
- waypoints = [current_wp]
- # 增加采样点到5个，更准确的判断弯道大小
- for i in range(5):
- next_wps = waypoints[-1].next(1.0)
- if next_wps:
- waypoints.append(next_wps[0])
- else:
- break
-
- if len(waypoints) < 4:
- return 0.0, 0
-
- # 计算每个相邻点的方向
- dirs = []
- for i in range(1, len(waypoints)):
- wp_prev = waypoints[i-1]
- wp_curr = waypoints[i]
- dir_rad = math.atan2(
- wp_curr.transform.location.y - wp_prev.transform.location.y,
- wp_curr.transform.location.x - wp_prev.transform.location.x
- )
- dirs.append(dir_rad)
-
- # 计算方向变化的绝对值之和（放大差异）
- dir_change = 0.0
- for i in range(1, len(dirs)):
- dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异
-
- # 划分弯道等级
- if dir_change < DIR_CHANGE_GENTLE:
- curve_level = 0 # 直道
- elif dir_change < DIR_CHANGE_SHARP:
- curve_level = 1 # 缓弯
- else:
- curve_level = 2 # 急弯
-
- return dir_change, curve_level
-
-# ===================== 相机管理器 =====================
-class CameraManager:
- def __init__(self, world, vehicle, display):
+# ===================== Pygame显示（优化版）=====================
+class PygameDisplay:
+ """Pygame摄像头显示类"""
+ def __init__(self, world, vehicle):
self.world = world
self.vehicle = vehicle
- self.display = display
self.camera = None
- self._create_camera()
-
- def _create_camera(self):
- bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
- bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
- bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
- bp.set_attribute("fov", str(CAMERA_FOV))
- self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
- self.camera.listen(self._on_image)
-
- def _on_image(self, image):
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3]
- array = array[:, :, ::-1].swapaxes(0, 1)
- self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
+ self.screen = None
+ self.surface = None
+
+ pygame.init()
+ self.screen = pygame.display.set_mode((800, 600))
+ pygame.display.set_caption("CARLA Camera View (ESC to quit)")
+
+ self._setup_camera()
+
+ def _setup_camera(self):
+ try:
+ bp_lib = self.world.get_blueprint_library()
+ camera_bp = bp_lib.find('sensor.camera.rgb')
+ camera_bp.set_attribute('image_size_x', '800')
+ camera_bp.set_attribute('image_size_y', '600')
+ camera_bp.set_attribute('fov', '90')
+ camera_transform = carla.Transform(carla.Location(x=2.5, z=1.8))
+ self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle)
+ self.camera.listen(lambda image: self._process_image(image))
+ except Exception as e:
+ logging.warning(f"摄像头初始化失败：{e}")
+ self.camera = None
+
+ def _process_image(self, image):
+ try:
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((image.height, image.width, 4))[:, :, :3]
+ array = array[:, :, ::-1].swapaxes(0, 1)
+ self.surface = pygame.surfarray.make_surface(array)
+ except Exception as e:
+ logging.warning(f"图像处理失败：{e}")
+
+ def parse_events(self):
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
+ return True
+ return False
+
+ def render(self):
+ self.screen.fill((0, 0, 0))
+ if self.surface is not None:
+ self.screen.blit(self.surface, (0, 0))
pygame.display.flip()
def destroy(self):
if self.camera:
self.camera.stop()
self.camera.destroy()
+ pygame.quit()
-# ===================== 主函数（核心逻辑）=====================
-def main():
- pygame.init()
- display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
- pygame.display.set_caption("CARLA 弯道自适应转向（最终版）")
-
- client = None
- world = None
- vehicle = None
- camera_manager = None
- pp_controller = None
- speed_controller = None
-
- try:
- # 1. 连接CARLA并初始化
- client = carla.Client(CARLA_HOST, CARLA_PORT)
- client.set_timeout(CARLA_TIMEOUT)
- world = client.get_world()
+# ===================== 绘图类（优化版）=====================
+class Plotter:
+ """轨迹+速度绘图类"""
+ def __init__(self, waypoints):
+ self.waypoints = waypoints
+ self.is_initialized = False
+ self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(14, 6))
+ self.x_data = []
+ self.y_data = []
+ self.time_data = []
+ self.speed_data = []
+ self.target_speed_data = []
+
+ def init_plot(self):
+ try:
+ waypoints_np = np.array(self.waypoints)
+ self.ax1.plot(waypoints_np[:, 0], waypoints_np[:, 1], 'ro-', label='Waypoints', markersize=4)
+ self.ax1.set_xlabel('X (m)')
+ self.ax1.set_ylabel('Y (m)')
+ self.ax1.set_title('Vehicle Trajectory')
+ self.ax1.legend()
+ self.ax1.grid(True)
+
+ self.ax2.set_xlabel('Time (s)')
+ self.ax2.set_ylabel('Speed (km/h)')
+ self.ax2.set_title('Speed vs Time')
+ self.ax2.grid(True)
+
+ plt.ion()
+ plt.show(block=False)
+ self.is_initialized = True
+ except Exception as e:
+ logging.error(f"绘图初始化失败：{e}")
+ self.is_initialized = False
+
+ def update_plot(self, time, x, y, speed, target_speed):
+ if not self.is_initialized:
+ return
+
+ try:
+ self.x_data.append(x)
+ self.y_data.append(y)
+ self.time_data.append(time)
+ self.speed_data.append(speed)
+ self.target_speed_data.append(target_speed)
+
+ self.ax1.plot(self.x_data, self.y_data, 'b-', label='Trajectory' if len(self.x_data) == 1 else "", linewidth=1)
+ if len(self.x_data) == 1:
+ self.ax1.legend()
+
+ self.ax2.plot(self.time_data, self.speed_data, 'b-', label='Current Speed' if len(self.time_data) == 1 else "", linewidth=1)
+ self.ax2.plot(self.time_data, self.target_speed_data, 'r--', label='Target Speed' if len(self.time_data) == 1 else "", linewidth=1)
+ if len(self.time_data) == 1:
+ self.ax2.legend()
+
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
+ except Exception as e:
+ logging.warning(f"绘图更新失败：{e}")
+
+ def cleanup_plot(self):
+ if self.is_initialized:
+ plt.ioff()
+ plt.close(self.fig)
+ self.is_initialized = False
+
+# ===================== 配置类（动态航点版）=====================
+class Config:
+ """仿真配置类"""
+ # 动态航点配置
+ NUM_WAYPOINTS = 50 # 生成的航点数量
+ WAYPOINT_SPACING = 2.0 # 航点间距（米）
+ TARGET_SPEED = 30.0 # 统一目标速度（km/h）
+
+ # 控制器参数
+ WAYPOINT_THRESHOLD = 1.5
+ MIN_SPEED_STANLEY = 1e-4
+ STANLEY_K = 0.4
+ MAX_STEER_DEG = 40.0
+ MAX_STEER_RAD = math.radians(MAX_STEER_DEG)
+
+ # CARLA配置
+ CARLA_HOST = "localhost"
+ CARLA_PORT = 2000
+ CARLA_TIMEOUT = 10.0
+
+ # 车辆配置（Town03主干道生成位置）
+ VEHICLE_MODEL = "vehicle.tesla.model3"
+ SPAWN_LOCATION = carla.Location(x=100.0, y=10.0, z=0.5) # Town03开阔主干道
+ SPAWN_YAW = 0.0 # 初始朝向
+
+ # 观察者视角（适配新生成位置）
+ SPECTATOR_LOCATION = carla.Location(x=110.0, y=10.0, z=8.0)
+ SPECTATOR_ROTATION = carla.Rotation(pitch=-30.0, yaw=0.0, roll=0.0)
+
+# ===================== 工具类（优化版）=====================
+class Utils:
+ """工具函数类"""
+
+ @staticmethod
+ def normalize_angle(angle: float) -> float:
+ while angle > np.pi:
+ angle -= 2 * np.pi
+ while angle < -np.pi:
+ angle += 2 * np.pi
+ return angle
+
+ @staticmethod
+ def get_vehicle_speed(vehicle: carla.Vehicle, unit: str = "kmh") -> float:
+ vel = vehicle.get_velocity()
+ speed_mps = np.linalg.norm([vel.x, vel.y, vel.z])
+ return speed_mps * 3.6 if unit == "kmh" else speed_mps
+
+ @staticmethod
+ def calculate_cte(vehicle_loc: carla.Location, prev_wp: List[float], target_wp: List[float]) -> float:
+ x1, y1 = prev_wp[0], prev_wp[1]
+ x2, y2 = target_wp[0], target_wp[1]
+ x, y = vehicle_loc.x, vehicle_loc.y
+
+ dx = x2 - x1
+ dy = y2 - y1
+ if abs(dx) < 1e-6 and abs(dy) < 1e-6:
+ return math.hypot(x - x1, y - y1)
+
+ if abs(dx) < 1e-6:
+ cte = x - x1
+ else:
+ slope = dy / dx
+ a = -slope
+ b = 1.0
+ c = slope * x1 - y1
+ cte = (a * x + b * y + c) / np.sqrt(a ** 2 + b ** 2)
+
+ yaw_path = np.arctan2(dy, dx)
+ yaw_ct = np.arctan2(y - y1, x - x1)
+ yaw_diff = Utils.normalize_angle(yaw_path - yaw_ct)
+
+ return abs(cte) if yaw_diff > 0 else -abs(cte)
+
+ @staticmethod
+ def calculate_distance_2d(loc1: carla.Location, loc2: carla.Location) -> float:
+ dx = loc1.x - loc2.x
+ dy = loc1.y - loc2.y
+ return math.sqrt(dx ** 2 + dy ** 2)
+
+ @staticmethod
+ def generate_road_waypoints(world, start_loc: carla.Location, num_waypoints: int, spacing: float) -> List[List[float]]:
+ """
+ 沿道路动态生成航点（核心：避免障碍物）
+ :param world: CARLA世界对象
+ :param start_loc: 起始位置
+ :param num_waypoints: 航点数量
+ :param spacing: 航点间距
+ :return: 航点列表 [[x, y, speed], ...]
+ """
map = world.get_map()
+ waypoints = []
+ current_waypoint = map.get_waypoint(start_loc)
+
+ for i in range(num_waypoints):
+ # 添加当前航点（x, y, 目标速度）
+ waypoints.append([current_waypoint.transform.location.x, current_waypoint.transform.location.y, Config.TARGET_SPEED])
+ # 获取下一个道路航点（沿道路前进）
+ next_waypoints = current_waypoint.next(spacing)
+ if not next_waypoints:
+ break
+ current_waypoint = next_waypoints[0]
+
+ logging.info(f"动态生成了 {len(waypoints)} 个道路航点")
+ return waypoints
+
+# ===================== 航点管理器（优化版）=====================
+class WaypointManager:
+ """航点管理类"""
+ def __init__(self, waypoints: List[List[float]], threshold: float):
+ self.waypoints = waypoints
+ self.threshold = threshold
+ self.current_target_id = 1
+
+ def update_target(self, vehicle_loc: carla.Location) -> None:
+ if self.current_target_id >= len(self.waypoints) - 1:
+ return
+
+ target_wp = self.waypoints[self.current_target_id]
+ target_loc = carla.Location(x=target_wp[0], y=target_wp[1])
+ distance = Utils.calculate_distance_2d(vehicle_loc, target_loc)
+
+ if distance < self.threshold:
+ self._log_waypoint_switch(distance)
+ self.current_target_id += 1
+
+ def _log_waypoint_switch(self, distance: float) -> None:
+ reached_id = self.current_target_id
+ reached_coords = (self.waypoints[reached_id][0], self.waypoints[reached_id][1])
+ log_msg = f"到达航点 {reached_id} (X={reached_coords[0]:.1f}, Y={reached_coords[1]:.1f}, 距离={distance:.1f}m)"
+
+ if self.current_target_id + 1 < len(self.waypoints):
+ next_id = self.current_target_id + 1
+ next_coords = (self.waypoints[next_id][0], self.waypoints[next_id][1])
+ log_msg += f"，新目标：航点 {next_id} (X={next_coords[0]:.1f}, Y={next_coords[1]:.1f})"
+
+ logging.info(log_msg)
+
+ def get_current_target(self) -> List[float]:
+ return self.waypoints[self.current_target_id]
+
+ def get_target_speed(self) -> float:
+ return self.get_current_target()[2] if self.current_target_id < len(self.waypoints) else 0.0
+
+# ===================== Stanley控制器（优化版）=====================
+class StanleyController:
+ """Stanley横向控制器"""
+ def __init__(self, k: float, max_steer_rad: float, min_speed: float):
+ self.k = k
+ self.max_steer_rad = max_steer_rad
+ self.min_speed = min_speed
+ self.prev_steer = 0.0
+ self.smoothing_factor = 0.7
+
+ def calculate_steer(self, vehicle: carla.Vehicle, waypoints: List[List[float]], target_id: int) -> Tuple[float, float]:
+ if target_id < 1:
+ return 0.0, 0.0
+
+ vehicle_transform = vehicle.get_transform()
+ vehicle_loc = vehicle_transform.location
+ vehicle_yaw = math.radians(vehicle_transform.rotation.yaw)
+ vehicle_speed = Utils.get_vehicle_speed(vehicle, unit="mps")
+
+ prev_wp = waypoints[target_id - 1]
+ target_wp = waypoints[target_id]
+
+ yaw_path = np.arctan2(target_wp[1] - prev_wp[1], target_wp[0] - prev_wp[0])
+ yaw_error = Utils.normalize_angle(yaw_path - vehicle_yaw)
+ cte = Utils.calculate_cte(vehicle_loc, prev_wp, target_wp)
+
+ # 动态K值
+ if vehicle_speed < 5:
+ k = self.k * 2
+ elif vehicle_speed > 15:
+ k = self.k * 0.5
+ else:
+ k = self.k
+
+ safe_speed = max(vehicle_speed, self.min_speed)
+ cte_steer = np.arctan(k * cte / safe_speed)
+
+ total_steer = Utils.normalize_angle(yaw_error + cte_steer)
+ total_steer = np.clip(total_steer, -self.max_steer_rad, self.max_steer_rad)
+ steer_carla = total_steer / self.max_steer_rad
+
+ # 平滑转向
+ steer_carla = self.smoothing_factor * self.prev_steer + (1 - self.smoothing_factor) * steer_carla
+ self.prev_steer = steer_carla
+
+ return steer_carla, cte
+
+# ===================== 主仿真类（动态航点版）=====================
+class CarlaSimulation:
+ """CARLA仿真主类"""
+ def __init__(self):
+ logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s")
+ self.logger = logging.getLogger(__name__)
+
+ self.config = Config()
+ self.client: Optional[carla.Client] = None
+ self.world: Optional[carla.World] = None
+ self.vehicle: Optional[carla.Vehicle] = None
+
+ # 动态航点（后续生成）
+ self.waypoints = []
+ self.waypoint_manager = None
+ self.stanley_controller = StanleyController(
+ k=self.config.STANLEY_K,
+ max_steer_rad=self.config.MAX_STEER_RAD,
+ min_speed=self.config.MIN_SPEED_STANLEY
+ )
+ self.pid_controller = PIDController()
+
+ self.plotter: Optional[Plotter] = None
+ self.pygame_display: Optional[PygameDisplay] = None
+ self.sim_start_time: float = 0.0
+
+ def connect_carla(self) -> bool:
+ try:
+ self.client = carla.Client(self.config.CARLA_HOST, self.config.CARLA_PORT)
+ self.client.set_timeout(self.config.CARLA_TIMEOUT)
+ self.world = self.client.get_world()
+ self.logger.info(f"成功连接到CARLA世界：{self.world.get_map().name}")
+ return True
+ except Exception as e:
+ self.logger.error(f"连接CARLA失败：{e}")
+ return False
+
+ def cleanup_vehicles(self) -> None:
+ self.logger.info("清理历史车辆...")
+ actors = self.world.get_actors().filter("vehicle.*")
+ removed_count = 0
+ for actor in actors:
+ if actor.attributes.get("role_name") == "my_car":
+ if actor.destroy():
+ removed_count += 1
+ self.logger.info(f"销毁车辆：{actor.type_id} (ID: {actor.id})")
+ self.logger.info(f"共销毁 {removed_count} 辆历史车辆")
+
+ def spawn_vehicle(self) -> bool:
+ """在开阔道路生成车辆（避开障碍物）"""
+ # 获取生成位置的道路Waypoint
+ map = self.world.get_map()
+ spawn_waypoint = map.get_waypoint(self.config.SPAWN_LOCATION)
+ if not spawn_waypoint:
+ self.logger.error("生成位置不在道路上")
+ return False
+
+ # 生成Transform
+ spawn_transform = spawn_waypoint.transform
+ spawn_transform.location.z += 0.5
+ spawn_transform.rotation.yaw = self.config.SPAWN_YAW
+
+ # 加载车辆蓝图
+ bp_lib = self.world.get_blueprint_library()
+ vehicle_bp = bp_lib.filter(self.config.VEHICLE_MODEL)[0]
+ vehicle_bp.set_attribute("role_name", "my_car")
+
+ # 生成车辆（重试）
+ self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform)
+ if self.vehicle is None:
+ spawn_transform.location.z += 0.5
+ self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform)
+ if self.vehicle is None:
+ self.logger.error(f"无法生成车辆：{spawn_transform.location}")
+ return False
+
+ # 动态生成道路航点（核心：避开障碍物）
+ self.waypoints = Utils.generate_road_waypoints(
+ world=self.world,
+ start_loc=self.config.SPAWN_LOCATION,
+ num_waypoints=self.config.NUM_WAYPOINTS,
+ spacing=self.config.WAYPOINT_SPACING
+ )
+ if len(self.waypoints) < 2:
+ self.logger.error("生成的航点数量不足")
+ return False
+
+ # 初始化航点管理器
+ self.waypoint_manager = WaypointManager(
+ waypoints=self.waypoints,
+ threshold=self.config.WAYPOINT_THRESHOLD
+ )
- # 清理现有车辆
- for actor in world.get_actors().filter("vehicle.*"):
- actor.destroy()
-
- # 生成车辆
- vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
- spawn_points = map.get_spawn_points()
- vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])
- print(f"车辆生成成功：{vehicle.type_id}")
-
- # 初始化控制器
- pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
- speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
- camera_manager = CameraManager(world, vehicle, display)
-
- # 2. 主循环
- print("仿真启动，按ESC退出...")
- clock = pygame.time.Clock()
- running = True
-
- while running:
- # 事件处理
- for event in pygame.event.get():
- if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
- running = False
-
- # 3. 获取关键数据
- vehicle_transform = vehicle.get_transform()
- vehicle_vel = vehicle.get_velocity()
- current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h
-
- # 获取当前Waypoint
- current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True)
-
- # 计算方向变化量和弯道等级
- dir_change, curve_level = calculate_dir_change(current_wp)
-
- # 动态获取预瞄距离
- lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)
-
- # 获取目标Waypoint（动态预瞄距离）
- target_wps = current_wp.next(lookahead_dist)
- target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
-
- # 4. 计算目标速度（根据弯道等级调整）
- curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数
- speed_factor = curve_speed_factors[min(curve_level, 2)]
- target_speed = BASE_SPEED * speed_factor
- target_speed = max(10.0, target_speed)
-
- # 5. 计算转向角（传入方向变化量，实现自适应）
- steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
-
- # 6. 计算油门/刹车
- throttle = speed_controller.calculate(target_speed, current_speed)
- brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0
+ self.logger.info(f"成功生成车辆：{self.vehicle.type_id} (ID: {self.vehicle.id})")
+ self.logger.info(f"生成位置：{spawn_transform.location}, 初始朝向：{spawn_transform.rotation.yaw:.1f}度")
+ return True
- # 7. 应用控制
+ def setup_spectator(self) -> None:
+ """设置观察者视角（清晰查看车辆前方）"""
+ spectator = self.world.get_spectator()
+ spectator_transform = carla.Transform(
+ self.config.SPECTATOR_LOCATION,
+ self.config.SPECTATOR_ROTATION
+ )
+ spectator.set_transform(spectator_transform)
+ self.logger.info("已设置观察者视角")
+
+ def init_visualization(self) -> None:
+ self.plotter = Plotter(waypoints=self.waypoints)
+ self.plotter.init_plot()
+ self.logger.info("Plotter初始化完成")
+
+ self.pygame_display = PygameDisplay(self.world, self.vehicle)
+ self.logger.info("Pygame显示初始化完成")
+
+ def run_simulation(self) -> None:
+ self.sim_start_time = time.time()
+ self.logger.info("开始仿真循环...")
+
+ while True:
+ if self.pygame_display.parse_events():
+ self.logger.info("用户请求退出仿真")
+ break
+
+ self.world.wait_for_tick()
+
+ # 获取车辆状态
+ vehicle_transform = self.vehicle.get_transform()
+ vehicle_loc = vehicle_transform.location
+ current_speed = Utils.get_vehicle_speed(vehicle=self.vehicle, unit="kmh")
+ sim_time = time.time() - self.sim_start_time
+
+ # 更新航点
+ self.waypoint_manager.update_target(vehicle_loc)
+ target_speed = self.waypoint_manager.get_target_speed()
+
+ # 计算控制
+ throttle, brake = self.pid_controller.calculate_control(target_speed, current_speed)
+ steer, cte = self.stanley_controller.calculate_steer(
+ vehicle=self.vehicle,
+ waypoints=self.waypoints,
+ target_id=self.waypoint_manager.current_target_id
+ )
+
+ # 应用控制
control = carla.VehicleControl()
- control.steer = steer
control.throttle = throttle
control.brake = brake
+ control.steer = steer
control.hand_brake = False
- vehicle.apply_control(control)
-
- # 8. 打印状态（调试用，显示弯道等级和动态参数）
- curve_names = ["直道", "缓弯", "急弯"]
- print(f"速度：{current_speed:.1f}km/h | 目标速度：{target_speed:.1f}km/h | 弯道：{curve_names[curve_level]} | 预瞄：{lookahead_dist:.1f}m | 转向：{steer:.3f}")
-
- # 控制帧率
- clock.tick(30)
-
- except Exception as e:
- print(f"错误：{e}")
- traceback.print_exc()
-
- finally:
- # 清理资源
- print("清理资源...")
- if camera_manager:
- camera_manager.destroy()
- if vehicle:
- vehicle.destroy()
- pygame.quit()
- print("仿真结束")
+ control.manual_gear_shift = False
+ self.vehicle.apply_control(control)
+
+ # 调试日志
+ if int(sim_time) % 1 == 0 and not hasattr(self, f"_logged_{int(sim_time)}"):
+ setattr(self, f"_logged_{int(sim_time)}", True)
+ self.logger.info(
+ f"时间：{sim_time:.1f}s | "
+ f"速度：{current_speed:.1f}km/h | "
+ f"目标速度：{target_speed:.1f}km/h | "
+ f"转向角：{steer:.2f} | "
+ f"横向偏差：{cte:.2f}m"
+ )
+
+ # 更新可视化
+ if self.plotter and self.plotter.is_initialized:
+ try:
+ self.plotter.update_plot(sim_time, vehicle_loc.x, vehicle_loc.y, current_speed, target_speed)
+ except Exception as e:
+ self.logger.warning(f"绘图更新失败：{e}")
+ self.plotter.cleanup_plot()
+ self.plotter = None
+
+ if self.pygame_display:
+ self.pygame_display.render()
+
+ def cleanup(self) -> None:
+ self.logger.info("开始清理资源...")
+
+ if self.pygame_display:
+ self.pygame_display.destroy()
+ self.logger.info("Pygame显示已销毁")
+
+ if self.plotter and self.plotter.is_initialized:
+ self.plotter.cleanup_plot()
+ self.logger.info("绘图器已清理")
+
+ if self.vehicle and self.vehicle.is_alive:
+ self.vehicle.destroy()
+ self.logger.info("车辆已销毁")
+
+ self.logger.info("资源清理完成")
+
+ def start(self) -> None:
+ try:
+ if not self.connect_carla():
+ return
+
+ self.cleanup_vehicles()
+
+ if not self.spawn_vehicle():
+ return
+
+ self.setup_spectator()
+ self.init_visualization()
+ self.run_simulation()
+
+ except KeyboardInterrupt:
+ self.logger.info("用户中断仿真")
+ except Exception as e:
+ self.logger.error(f"仿真异常：{e}", exc_info=True)
+ finally:
+ self.cleanup()
+
+# ===================== 主函数 =====================
+def main():
+ simulation = CarlaSimulation()
+ simulation.start()
if __name__ == "__main__":
main()
\ No newline at end of file
From 3edfc11fe39c511973bf6aa1eea00e13981c7456 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sat, 20 Dec 2025 12:29:37 +0800
Subject: [PATCH 11/26] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E8=BD=A6=E8=BE=86?=
=?UTF-8?q?=E8=87=AA=E4=B8=BB=E9=81=93=E8=B7=AF=E8=88=AA=E7=82=B9=E8=B7=9F?=
=?UTF-8?q?=E9=9A=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 826 ++++++++++-------------------
1 file changed, 290 insertions(+), 536 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 64f88ed002..2e6e2fd2f2 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,214 +1,48 @@
#!/usr/bin/env python
+# -*- coding: utf-8 -*-
"""
-CARLA Waypoint Following - Dynamic Road Waypoints (sd_6/__main__.py)
-
-核心改进：
-1. 动态生成沿道路的航点，替代固定坐标，避开障碍物
-2. 选择地图开阔区域生成车辆（Town03主干道）
-3. 增加障碍物检测，确保行驶路段安全
-4. 优化航点跟踪逻辑，适配动态道路
+CARLA Waypoint Following（原生道路航点版）
+核心：直接使用CARLA地图的道路航点，车辆100%沿道路行驶
"""
-# ===================== 系统导入 & 路径配置 =====================
import sys
import os
import carla
import numpy as np
-import time
import math
-import logging
-from typing import List, Tuple, Optional, Union
-import pygame
import matplotlib.pyplot as plt
+import cv2
+import time
-# 获取当前脚本目录并加入搜索路径
-current_dir = os.path.dirname(os.path.abspath(__file__))
-sys.path.append(current_dir)
-
-# ===================== PID控制器（优化版）=====================
-class PIDController:
- """PID速度控制器"""
- def __init__(self):
- self.kp = 0.3
- self.ki = 0.01
- self.kd = 0.1
- self.prev_error = 0.0
- self.integral = 0.0
- self.integral_limit = 0.5
-
- def calculate_control(self, target_speed, current_speed):
- error = target_speed - current_speed
- self.integral = np.clip(self.integral + error * self.ki, -self.integral_limit, self.integral_limit)
- derivative = (error - self.prev_error) * self.kd
- output = self.kp * error + self.integral + derivative
-
- throttle = max(0.0, min(1.0, output)) if output > 0 else 0.0
- brake = max(0.0, min(1.0, -output)) if output < 0 else 0.0
-
- self.prev_error = error
-
- from collections import namedtuple
- Control = namedtuple('Control', ['throttle', 'brake'])
- return Control(throttle=throttle, brake=brake)
-
-# ===================== Pygame显示（优化版）=====================
-class PygameDisplay:
- """Pygame摄像头显示类"""
- def __init__(self, world, vehicle):
- self.world = world
- self.vehicle = vehicle
- self.camera = None
- self.screen = None
- self.surface = None
-
- pygame.init()
- self.screen = pygame.display.set_mode((800, 600))
- pygame.display.set_caption("CARLA Camera View (ESC to quit)")
-
- self._setup_camera()
-
- def _setup_camera(self):
- try:
- bp_lib = self.world.get_blueprint_library()
- camera_bp = bp_lib.find('sensor.camera.rgb')
- camera_bp.set_attribute('image_size_x', '800')
- camera_bp.set_attribute('image_size_y', '600')
- camera_bp.set_attribute('fov', '90')
- camera_transform = carla.Transform(carla.Location(x=2.5, z=1.8))
- self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle)
- self.camera.listen(lambda image: self._process_image(image))
- except Exception as e:
- logging.warning(f"摄像头初始化失败：{e}")
- self.camera = None
-
- def _process_image(self, image):
- try:
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = array.reshape((image.height, image.width, 4))[:, :, :3]
- array = array[:, :, ::-1].swapaxes(0, 1)
- self.surface = pygame.surfarray.make_surface(array)
- except Exception as e:
- logging.warning(f"图像处理失败：{e}")
-
- def parse_events(self):
- for event in pygame.event.get():
- if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
- return True
- return False
-
- def render(self):
- self.screen.fill((0, 0, 0))
- if self.surface is not None:
- self.screen.blit(self.surface, (0, 0))
- pygame.display.flip()
-
- def destroy(self):
- if self.camera:
- self.camera.stop()
- self.camera.destroy()
- pygame.quit()
-
-# ===================== 绘图类（优化版）=====================
-class Plotter:
- """轨迹+速度绘图类"""
- def __init__(self, waypoints):
- self.waypoints = waypoints
- self.is_initialized = False
- self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(14, 6))
- self.x_data = []
- self.y_data = []
- self.time_data = []
- self.speed_data = []
- self.target_speed_data = []
-
- def init_plot(self):
- try:
- waypoints_np = np.array(self.waypoints)
- self.ax1.plot(waypoints_np[:, 0], waypoints_np[:, 1], 'ro-', label='Waypoints', markersize=4)
- self.ax1.set_xlabel('X (m)')
- self.ax1.set_ylabel('Y (m)')
- self.ax1.set_title('Vehicle Trajectory')
- self.ax1.legend()
- self.ax1.grid(True)
-
- self.ax2.set_xlabel('Time (s)')
- self.ax2.set_ylabel('Speed (km/h)')
- self.ax2.set_title('Speed vs Time')
- self.ax2.grid(True)
-
- plt.ion()
- plt.show(block=False)
- self.is_initialized = True
- except Exception as e:
- logging.error(f"绘图初始化失败：{e}")
- self.is_initialized = False
-
- def update_plot(self, time, x, y, speed, target_speed):
- if not self.is_initialized:
- return
-
- try:
- self.x_data.append(x)
- self.y_data.append(y)
- self.time_data.append(time)
- self.speed_data.append(speed)
- self.target_speed_data.append(target_speed)
-
- self.ax1.plot(self.x_data, self.y_data, 'b-', label='Trajectory' if len(self.x_data) == 1 else "", linewidth=1)
- if len(self.x_data) == 1:
- self.ax1.legend()
-
- self.ax2.plot(self.time_data, self.speed_data, 'b-', label='Current Speed' if len(self.time_data) == 1 else "", linewidth=1)
- self.ax2.plot(self.time_data, self.target_speed_data, 'r--', label='Target Speed' if len(self.time_data) == 1 else "", linewidth=1)
- if len(self.time_data) == 1:
- self.ax2.legend()
-
- self.fig.canvas.draw()
- self.fig.canvas.flush_events()
- except Exception as e:
- logging.warning(f"绘图更新失败：{e}")
-
- def cleanup_plot(self):
- if self.is_initialized:
- plt.ioff()
- plt.close(self.fig)
- self.is_initialized = False
-
-# ===================== 配置类（动态航点版）=====================
+# ===================== 核心配置 =====================
class Config:
- """仿真配置类"""
- # 动态航点配置
- NUM_WAYPOINTS = 50 # 生成的航点数量
- WAYPOINT_SPACING = 2.0 # 航点间距（米）
- TARGET_SPEED = 30.0 # 统一目标速度（km/h）
-
- # 控制器参数
- WAYPOINT_THRESHOLD = 1.5
- MIN_SPEED_STANLEY = 1e-4
- STANLEY_K = 0.4
- MAX_STEER_DEG = 40.0
- MAX_STEER_RAD = math.radians(MAX_STEER_DEG)
-
- # CARLA配置
- CARLA_HOST = "localhost"
- CARLA_PORT = 2000
- CARLA_TIMEOUT = 10.0
-
- # 车辆配置（Town03主干道生成位置）
+ # 速度控制
+ TARGET_SPEED = 20.0 # km/h
+ PID_KP = 0.3
+ PID_KI = 0.02
+ PID_KD = 0.01
+
+ # 纯追踪算法参数
+ LOOKAHEAD_DISTANCE = 8.0 # 前瞻距离（米）
+ MAX_STEER_ANGLE = 30.0 # 最大转向角（度）
+
+ # 摄像头配置
+ CAMERA_WIDTH = 800
+ CAMERA_HEIGHT = 600
+ CAMERA_FOV = 90
+
+ # 车辆配置
VEHICLE_MODEL = "vehicle.tesla.model3"
- SPAWN_LOCATION = carla.Location(x=100.0, y=10.0, z=0.5) # Town03开阔主干道
- SPAWN_YAW = 0.0 # 初始朝向
-
- # 观察者视角（适配新生成位置）
- SPECTATOR_LOCATION = carla.Location(x=110.0, y=10.0, z=8.0)
- SPECTATOR_ROTATION = carla.Rotation(pitch=-30.0, yaw=0.0, roll=0.0)
-# ===================== 工具类（优化版）=====================
-class Utils:
- """工具函数类"""
+ # 可视化配置
+ PLOT_SIZE = (12, 10)
+ WAYPOINT_COUNT = 50 # 预先生成的道路航点数量
+# ===================== 工具类 =====================
+class Tools:
@staticmethod
- def normalize_angle(angle: float) -> float:
+ def normalize_angle(angle):
+ """将角度归一化到[-pi, pi]"""
while angle > np.pi:
angle -= 2 * np.pi
while angle < -np.pi:
@@ -216,372 +50,292 @@ def normalize_angle(angle: float) -> float:
return angle
@staticmethod
- def get_vehicle_speed(vehicle: carla.Vehicle, unit: str = "kmh") -> float:
+ def get_vehicle_pose(vehicle):
+ """获取车辆的位置、朝向和速度"""
+ transform = vehicle.get_transform()
+ loc = transform.location
+ yaw = math.radians(transform.rotation.yaw)
vel = vehicle.get_velocity()
- speed_mps = np.linalg.norm([vel.x, vel.y, vel.z])
- return speed_mps * 3.6 if unit == "kmh" else speed_mps
+ speed = 3.6 * np.linalg.norm([vel.x, vel.y, vel.z])
+ return loc, yaw, speed
@staticmethod
- def calculate_cte(vehicle_loc: carla.Location, prev_wp: List[float], target_wp: List[float]) -> float:
- x1, y1 = prev_wp[0], prev_wp[1]
- x2, y2 = target_wp[0], target_wp[1]
- x, y = vehicle_loc.x, vehicle_loc.y
-
- dx = x2 - x1
- dy = y2 - y1
- if abs(dx) < 1e-6 and abs(dy) < 1e-6:
- return math.hypot(x - x1, y - y1)
-
- if abs(dx) < 1e-6:
- cte = x - x1
- else:
- slope = dy / dx
- a = -slope
- b = 1.0
- c = slope * x1 - y1
- cte = (a * x + b * y + c) / np.sqrt(a ** 2 + b ** 2)
-
- yaw_path = np.arctan2(dy, dx)
- yaw_ct = np.arctan2(y - y1, x - x1)
- yaw_diff = Utils.normalize_angle(yaw_path - yaw_ct)
-
- return abs(cte) if yaw_diff > 0 else -abs(cte)
+ def clear_all_actors(world):
+ """清理所有车辆和传感器"""
+ for actor in world.get_actors():
+ try:
+ if actor.type_id.startswith('vehicle') or actor.type_id.startswith('sensor'):
+ actor.destroy()
+ except:
+ pass
+ time.sleep(0.5)
+ print("已清理所有残留Actor")
@staticmethod
- def calculate_distance_2d(loc1: carla.Location, loc2: carla.Location) -> float:
- dx = loc1.x - loc2.x
- dy = loc1.y - loc2.y
- return math.sqrt(dx ** 2 + dy ** 2)
+ def focus_vehicle(world, vehicle):
+ """将CARLA客户端视角聚焦到车辆"""
+ spectator = world.get_spectator()
+ t = vehicle.get_transform()
+ spectator.set_transform(carla.Transform(t.location + carla.Location(x=0, y=-8, z=5), t.rotation))
+ print("CARLA客户端已聚焦到车辆")
@staticmethod
- def generate_road_waypoints(world, start_loc: carla.Location, num_waypoints: int, spacing: float) -> List[List[float]]:
+ def generate_road_waypoints(world, start_loc, count=50, step=2.0):
"""
- 沿道路动态生成航点（核心：避免障碍物）
+ 从起点沿道路生成连续的原生航点
:param world: CARLA世界对象
- :param start_loc: 起始位置
- :param num_waypoints: 航点数量
- :param spacing: 航点间距
- :return: 航点列表 [[x, y, speed], ...]
+ :param start_loc: 起点位置
+ :param count: 航点数量
+ :param step: 每个航点的步长（米）
+ :return: 航点列表[(x, y, z), ...]
"""
- map = world.get_map()
waypoints = []
- current_waypoint = map.get_waypoint(start_loc)
-
- for i in range(num_waypoints):
- # 添加当前航点（x, y, 目标速度）
- waypoints.append([current_waypoint.transform.location.x, current_waypoint.transform.location.y, Config.TARGET_SPEED])
- # 获取下一个道路航点（沿道路前进）
- next_waypoints = current_waypoint.next(spacing)
- if not next_waypoints:
- break
- current_waypoint = next_waypoints[0]
-
- logging.info(f"动态生成了 {len(waypoints)} 个道路航点")
+ map = world.get_map()
+ wp = map.get_waypoint(start_loc)
+ for i in range(count):
+ waypoints.append((wp.transform.location.x, wp.transform.location.y, wp.transform.location.z))
+ # 沿道路下一个航点（直走，不考虑分叉）
+ wp = wp.next(step)[0]
+ print(f"生成了{len(waypoints)}个原生道路航点")
return waypoints
-# ===================== 航点管理器（优化版）=====================
-class WaypointManager:
- """航点管理类"""
- def __init__(self, waypoints: List[List[float]], threshold: float):
- self.waypoints = waypoints
- self.threshold = threshold
- self.current_target_id = 1
-
- def update_target(self, vehicle_loc: carla.Location) -> None:
- if self.current_target_id >= len(self.waypoints) - 1:
- return
-
- target_wp = self.waypoints[self.current_target_id]
- target_loc = carla.Location(x=target_wp[0], y=target_wp[1])
- distance = Utils.calculate_distance_2d(vehicle_loc, target_loc)
-
- if distance < self.threshold:
- self._log_waypoint_switch(distance)
- self.current_target_id += 1
-
- def _log_waypoint_switch(self, distance: float) -> None:
- reached_id = self.current_target_id
- reached_coords = (self.waypoints[reached_id][0], self.waypoints[reached_id][1])
- log_msg = f"到达航点 {reached_id} (X={reached_coords[0]:.1f}, Y={reached_coords[1]:.1f}, 距离={distance:.1f}m)"
-
- if self.current_target_id + 1 < len(self.waypoints):
- next_id = self.current_target_id + 1
- next_coords = (self.waypoints[next_id][0], self.waypoints[next_id][1])
- log_msg += f"，新目标：航点 {next_id} (X={next_coords[0]:.1f}, Y={next_coords[1]:.1f})"
-
- logging.info(log_msg)
-
- def get_current_target(self) -> List[float]:
- return self.waypoints[self.current_target_id]
-
- def get_target_speed(self) -> float:
- return self.get_current_target()[2] if self.current_target_id < len(self.waypoints) else 0.0
-
-# ===================== Stanley控制器（优化版）=====================
-class StanleyController:
- """Stanley横向控制器"""
- def __init__(self, k: float, max_steer_rad: float, min_speed: float):
- self.k = k
- self.max_steer_rad = max_steer_rad
- self.min_speed = min_speed
- self.prev_steer = 0.0
- self.smoothing_factor = 0.7
-
- def calculate_steer(self, vehicle: carla.Vehicle, waypoints: List[List[float]], target_id: int) -> Tuple[float, float]:
- if target_id < 1:
- return 0.0, 0.0
-
- vehicle_transform = vehicle.get_transform()
- vehicle_loc = vehicle_transform.location
- vehicle_yaw = math.radians(vehicle_transform.rotation.yaw)
- vehicle_speed = Utils.get_vehicle_speed(vehicle, unit="mps")
-
- prev_wp = waypoints[target_id - 1]
- target_wp = waypoints[target_id]
-
- yaw_path = np.arctan2(target_wp[1] - prev_wp[1], target_wp[0] - prev_wp[0])
- yaw_error = Utils.normalize_angle(yaw_path - vehicle_yaw)
- cte = Utils.calculate_cte(vehicle_loc, prev_wp, target_wp)
-
- # 动态K值
- if vehicle_speed < 5:
- k = self.k * 2
- elif vehicle_speed > 15:
- k = self.k * 0.5
- else:
- k = self.k
-
- safe_speed = max(vehicle_speed, self.min_speed)
- cte_steer = np.arctan(k * cte / safe_speed)
-
- total_steer = Utils.normalize_angle(yaw_error + cte_steer)
- total_steer = np.clip(total_steer, -self.max_steer_rad, self.max_steer_rad)
- steer_carla = total_steer / self.max_steer_rad
-
- # 平滑转向
- steer_carla = self.smoothing_factor * self.prev_steer + (1 - self.smoothing_factor) * steer_carla
- self.prev_steer = steer_carla
-
- return steer_carla, cte
-
-# ===================== 主仿真类（动态航点版）=====================
-class CarlaSimulation:
- """CARLA仿真主类"""
- def __init__(self):
- logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s")
- self.logger = logging.getLogger(__name__)
-
- self.config = Config()
- self.client: Optional[carla.Client] = None
- self.world: Optional[carla.World] = None
- self.vehicle: Optional[carla.Vehicle] = None
-
- # 动态航点（后续生成）
- self.waypoints = []
- self.waypoint_manager = None
- self.stanley_controller = StanleyController(
- k=self.config.STANLEY_K,
- max_steer_rad=self.config.MAX_STEER_RAD,
- min_speed=self.config.MIN_SPEED_STANLEY
- )
- self.pid_controller = PIDController()
-
- self.plotter: Optional[Plotter] = None
- self.pygame_display: Optional[PygameDisplay] = None
- self.sim_start_time: float = 0.0
-
- def connect_carla(self) -> bool:
- try:
- self.client = carla.Client(self.config.CARLA_HOST, self.config.CARLA_PORT)
- self.client.set_timeout(self.config.CARLA_TIMEOUT)
- self.world = self.client.get_world()
- self.logger.info(f"成功连接到CARLA世界：{self.world.get_map().name}")
- return True
- except Exception as e:
- self.logger.error(f"连接CARLA失败：{e}")
- return False
-
- def cleanup_vehicles(self) -> None:
- self.logger.info("清理历史车辆...")
- actors = self.world.get_actors().filter("vehicle.*")
- removed_count = 0
- for actor in actors:
- if actor.attributes.get("role_name") == "my_car":
- if actor.destroy():
- removed_count += 1
- self.logger.info(f"销毁车辆：{actor.type_id} (ID: {actor.id})")
- self.logger.info(f"共销毁 {removed_count} 辆历史车辆")
-
- def spawn_vehicle(self) -> bool:
- """在开阔道路生成车辆（避开障碍物）"""
- # 获取生成位置的道路Waypoint
- map = self.world.get_map()
- spawn_waypoint = map.get_waypoint(self.config.SPAWN_LOCATION)
- if not spawn_waypoint:
- self.logger.error("生成位置不在道路上")
- return False
-
- # 生成Transform
- spawn_transform = spawn_waypoint.transform
- spawn_transform.location.z += 0.5
- spawn_transform.rotation.yaw = self.config.SPAWN_YAW
-
- # 加载车辆蓝图
- bp_lib = self.world.get_blueprint_library()
- vehicle_bp = bp_lib.filter(self.config.VEHICLE_MODEL)[0]
- vehicle_bp.set_attribute("role_name", "my_car")
-
- # 生成车辆（重试）
- self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform)
- if self.vehicle is None:
- spawn_transform.location.z += 0.5
- self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform)
- if self.vehicle is None:
- self.logger.error(f"无法生成车辆：{spawn_transform.location}")
- return False
-
- # 动态生成道路航点（核心：避开障碍物）
- self.waypoints = Utils.generate_road_waypoints(
- world=self.world,
- start_loc=self.config.SPAWN_LOCATION,
- num_waypoints=self.config.NUM_WAYPOINTS,
- spacing=self.config.WAYPOINT_SPACING
- )
- if len(self.waypoints) < 2:
- self.logger.error("生成的航点数量不足")
- return False
-
- # 初始化航点管理器
- self.waypoint_manager = WaypointManager(
- waypoints=self.waypoints,
- threshold=self.config.WAYPOINT_THRESHOLD
- )
-
- self.logger.info(f"成功生成车辆：{self.vehicle.type_id} (ID: {self.vehicle.id})")
- self.logger.info(f"生成位置：{spawn_transform.location}, 初始朝向：{spawn_transform.rotation.yaw:.1f}度")
- return True
-
- def setup_spectator(self) -> None:
- """设置观察者视角（清晰查看车辆前方）"""
- spectator = self.world.get_spectator()
- spectator_transform = carla.Transform(
- self.config.SPECTATOR_LOCATION,
- self.config.SPECTATOR_ROTATION
- )
- spectator.set_transform(spectator_transform)
- self.logger.info("已设置观察者视角")
-
- def init_visualization(self) -> None:
- self.plotter = Plotter(waypoints=self.waypoints)
- self.plotter.init_plot()
- self.logger.info("Plotter初始化完成")
-
- self.pygame_display = PygameDisplay(self.world, self.vehicle)
- self.logger.info("Pygame显示初始化完成")
-
- def run_simulation(self) -> None:
- self.sim_start_time = time.time()
- self.logger.info("开始仿真循环...")
-
- while True:
- if self.pygame_display.parse_events():
- self.logger.info("用户请求退出仿真")
- break
-
- self.world.wait_for_tick()
-
- # 获取车辆状态
- vehicle_transform = self.vehicle.get_transform()
- vehicle_loc = vehicle_transform.location
- current_speed = Utils.get_vehicle_speed(vehicle=self.vehicle, unit="kmh")
- sim_time = time.time() - self.sim_start_time
-
- # 更新航点
- self.waypoint_manager.update_target(vehicle_loc)
- target_speed = self.waypoint_manager.get_target_speed()
-
- # 计算控制
- throttle, brake = self.pid_controller.calculate_control(target_speed, current_speed)
- steer, cte = self.stanley_controller.calculate_steer(
- vehicle=self.vehicle,
- waypoints=self.waypoints,
- target_id=self.waypoint_manager.current_target_id
- )
-
- # 应用控制
- control = carla.VehicleControl()
- control.throttle = throttle
- control.brake = brake
- control.steer = steer
- control.hand_brake = False
- control.manual_gear_shift = False
- self.vehicle.apply_control(control)
-
- # 调试日志
- if int(sim_time) % 1 == 0 and not hasattr(self, f"_logged_{int(sim_time)}"):
- setattr(self, f"_logged_{int(sim_time)}", True)
- self.logger.info(
- f"时间：{sim_time:.1f}s | "
- f"速度：{current_speed:.1f}km/h | "
- f"目标速度：{target_speed:.1f}km/h | "
- f"转向角：{steer:.2f} | "
- f"横向偏差：{cte:.2f}m"
- )
+# ===================== 摄像头回调 =====================
+def camera_callback(image, data_dict):
+ array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))[:, :, :3]
+ data_dict['image'] = array
+
+# ===================== 控制器 =====================
+class PIDSpeedController:
+ def __init__(self, config):
+ self.kp = config.PID_KP
+ self.ki = config.PID_KI
+ self.kd = config.PID_KD
+ self.prev_err = 0.0
+ self.integral = 0.0
+ self.target_speed = config.TARGET_SPEED
+
+ def calculate(self, current_speed):
+ err = self.target_speed - current_speed
+ self.integral = np.clip(self.integral + err * 0.05, -1.0, 1.0)
+ deriv = (err - self.prev_err) / 0.05
+ output = self.kp * err + self.ki * self.integral + self.kd * deriv
+ self.prev_err = err
+ return np.clip(output, 0.1, 1.0)
+
+class PurePursuitController:
+ def __init__(self, config):
+ self.lookahead_dist = config.LOOKAHEAD_DISTANCE
+ self.max_steer_rad = math.radians(config.MAX_STEER_ANGLE)
+
+ def calculate_steer(self, vehicle_loc, vehicle_yaw, waypoints):
+ """
+ 纯追踪算法计算转向角
+ :param vehicle_loc: 车辆位置
+ :param vehicle_yaw: 车辆朝向（弧度）
+ :param waypoints: 道路航点列表
+ :return: 转向角（-1~1）
+ """
+ # 1. 将航点转换为车辆坐标系
+ wp_coords = np.array(waypoints)
+ vehicle_x = vehicle_loc.x
+ vehicle_y = vehicle_loc.y
+
+ # 旋转和平移（车辆坐标系：x向前，y向左）
+ cos_yaw = math.cos(vehicle_yaw)
+ sin_yaw = math.sin(vehicle_yaw)
+ translated_x = wp_coords[:, 0] - vehicle_x
+ translated_y = wp_coords[:, 1] - vehicle_y
+ rotated_x = translated_x * cos_yaw + translated_y * sin_yaw
+ rotated_y = -translated_x * sin_yaw + translated_y * cos_yaw
+
+ # 2. 找到距离车辆>=前瞻距离的第一个航点
+ distances = np.hypot(rotated_x, rotated_y)
+ valid_wp_indices = np.where(distances >= self.lookahead_dist)[0]
+ if len(valid_wp_indices) == 0:
+ return 0.0
+
+ target_idx = valid_wp_indices[0]
+ target_x = rotated_x[target_idx]
+ target_y = rotated_y[target_idx]
+
+ # 3. 计算转向角（纯追踪公式：steer = arctan(2*L*y/(x²+y²))，L为车辆轴距，这里简化为1.0）
+ L = 1.0 # 车辆轴距（米）
+ steer_rad = math.atan2(2 * L * target_y, self.lookahead_dist ** 2)
+
+ # 4. 限制转向角
+ steer_rad = np.clip(steer_rad, -self.max_steer_rad, self.max_steer_rad)
+ steer = steer_rad / self.max_steer_rad
+
+ return steer
+
+# ===================== 可视化 =====================
+class Visualizer:
+ def __init__(self, config, spawn_loc, initial_waypoints):
+ self.waypoints = np.array(initial_waypoints)
+ self.trajectory = []
+ self.spawn_loc = (spawn_loc.x, spawn_loc.y)
+
+ plt.rcParams['backend'] = 'TkAgg'
+ plt.ioff()
+ self.fig, self.ax = plt.subplots(figsize=config.PLOT_SIZE)
+
+ # 绘制道路航点（CARLA原生）
+ self.ax.scatter(self.waypoints[:, 0], self.waypoints[:, 1], c='blue', s=50, label='Road Waypoints (CARLA)', zorder=3)
+ # 绘制生成点
+ self.ax.scatter(self.spawn_loc[0], self.spawn_loc[1], c='orange', marker='s', s=150, label='Spawn Point', zorder=5)
+ # 轨迹和车辆
+ self.traj_line, = self.ax.plot([], [], c='red', linewidth=4, label='Vehicle Trajectory', zorder=2)
+ self.vehicle_dot, = self.ax.plot([], [], c='green', marker='o', markersize=20, label='Vehicle', zorder=6)
+
+ self.ax.set_xlabel('X (m)', fontsize=14)
+ self.ax.set_ylabel('Y (m)', fontsize=14)
+ self.ax.set_title('CARLA Road Following (Native Waypoints)', fontsize=16)
+ self.ax.legend(fontsize=12)
+ self.ax.grid(True, alpha=0.3)
+ self.ax.axis('equal')
+
+ plt.show(block=False)
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
+
+ def update(self, vehicle_x, vehicle_y, new_waypoints=None):
+ """更新轨迹和航点"""
+ self.trajectory.append([vehicle_x, vehicle_y])
+ if len(self.trajectory) > 1000:
+ self.trajectory = self.trajectory[-1000:]
+
+ # 更新轨迹
+ traj = np.array(self.trajectory)
+ self.traj_line.set_data(traj[:, 0], traj[:, 1])
+ self.vehicle_dot.set_data(vehicle_x, vehicle_y)
+
+ # 更新航点（如果有新航点）
+ if new_waypoints is not None:
+ self.waypoints = np.array(new_waypoints)
+ self.ax.scatter(self.waypoints[:, 0], self.waypoints[:, 1], c='blue', s=50, zorder=3)
+
+ self.ax.relim()
+ self.ax.autoscale_view(True, True, True)
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
- # 更新可视化
- if self.plotter and self.plotter.is_initialized:
- try:
- self.plotter.update_plot(sim_time, vehicle_loc.x, vehicle_loc.y, current_speed, target_speed)
- except Exception as e:
- self.logger.warning(f"绘图更新失败：{e}")
- self.plotter.cleanup_plot()
- self.plotter = None
+# ===================== 主函数 =====================
+def main():
+ config = Config()
+ tools = Tools()
+
+ # 初始化OpenCV窗口
+ cv2.namedWindow('CARLA Vehicle View', cv2.WINDOW_NORMAL)
+ cv2.resizeWindow('CARLA Vehicle View', config.CAMERA_WIDTH, config.CAMERA_HEIGHT)
+
+ # 连接CARLA
+ try:
+ client = carla.Client('localhost', 2000)
+ client.set_timeout(30.0)
+ world = client.load_world('Town03')
+ map = world.get_map()
- if self.pygame_display:
- self.pygame_display.render()
+ # 设置同步模式
+ settings = world.get_settings()
+ settings.synchronous_mode = True
+ settings.fixed_delta_seconds = 0.05
+ world.apply_settings(settings)
+
+ # 清理Actor
+ tools.clear_all_actors(world)
+
+ # 获取道路生成点
+ spawn_points = map.get_spawn_points()
+ spawn_transform = spawn_points[0]
+ print(f"使用道路生成点：{spawn_transform.location}")
+
+ # 生成车辆
+ bp_lib = world.get_blueprint_library()
+ vehicle_bp = bp_lib.find(config.VEHICLE_MODEL)
+ vehicle = world.spawn_actor(vehicle_bp, spawn_transform)
+ if not vehicle:
+ print("车辆生成失败！")
+ return
+ print(f"车辆{config.VEHICLE_MODEL}生成成功")
- def cleanup(self) -> None:
- self.logger.info("开始清理资源...")
+ # 聚焦车辆
+ tools.focus_vehicle(world, vehicle)
- if self.pygame_display:
- self.pygame_display.destroy()
- self.logger.info("Pygame显示已销毁")
+ # 挂载摄像头
+ camera_bp = bp_lib.find('sensor.camera.rgb')
+ camera_bp.set_attribute('image_size_x', str(config.CAMERA_WIDTH))
+ camera_bp.set_attribute('image_size_y', str(config.CAMERA_HEIGHT))
+ camera_bp.set_attribute('fov', str(config.CAMERA_FOV))
+ camera = world.spawn_actor(camera_bp, carla.Transform(carla.Location(x=2.0, z=1.8)), attach_to=vehicle)
- if self.plotter and self.plotter.is_initialized:
- self.plotter.cleanup_plot()
- self.logger.info("绘图器已清理")
+ # 摄像头数据
+ camera_data = {'image': np.zeros((config.CAMERA_HEIGHT, config.CAMERA_WIDTH, 3), dtype=np.uint8)}
+ camera.listen(lambda img: camera_callback(img, camera_data))
- if self.vehicle and self.vehicle.is_alive:
- self.vehicle.destroy()
- self.logger.info("车辆已销毁")
+ # 生成初始道路航点
+ initial_waypoints = tools.generate_road_waypoints(world, spawn_transform.location, config.WAYPOINT_COUNT)
- self.logger.info("资源清理完成")
+ # 初始化控制器
+ speed_controller = PIDSpeedController(config)
+ path_controller = PurePursuitController(config)
- def start(self) -> None:
- try:
- if not self.connect_carla():
- return
+ # 初始化可视化
+ visualizer = Visualizer(config, spawn_transform.location, initial_waypoints)
- self.cleanup_vehicles()
+ # 主循环
+ while True:
+ world.tick()
- if not self.spawn_vehicle():
- return
+ # 获取车辆状态
+ vehicle_loc, vehicle_yaw, current_speed = tools.get_vehicle_pose(vehicle)
- self.setup_spectator()
- self.init_visualization()
- self.run_simulation()
+ # 实时更新道路航点（每10帧更新一次，减少计算量）
+ if world.get_snapshot().frame % 10 == 0:
+ new_waypoints = tools.generate_road_waypoints(world, vehicle_loc, config.WAYPOINT_COUNT)
+ else:
+ new_waypoints = None
- except KeyboardInterrupt:
- self.logger.info("用户中断仿真")
- except Exception as e:
- self.logger.error(f"仿真异常：{e}", exc_info=True)
- finally:
- self.cleanup()
+ # 更新可视化
+ visualizer.update(vehicle_loc.x, vehicle_loc.y, new_waypoints)
-# ===================== 主函数 =====================
-def main():
- simulation = CarlaSimulation()
- simulation.start()
+ # 显示摄像头画面
+ cv2.imshow('CARLA Vehicle View', camera_data['image'])
+ if cv2.waitKey(1) & 0xFF == ord('q'):
+ break
-if __name__ == "__main__":
+ # 计算控制量
+ steer = path_controller.calculate_steer(vehicle_loc, vehicle_yaw, initial_waypoints)
+ throttle = speed_controller.calculate(current_speed)
+ brake = 1.0 if current_speed > config.TARGET_SPEED * 2 else 0.0
+
+ # 控制车辆
+ vehicle.apply_control(carla.VehicleControl(
+ throttle=throttle,
+ brake=brake,
+ steer=steer,
+ hand_brake=False,
+ reverse=False
+ ))
+
+ # 打印状态
+ print(f"速度：{current_speed:.1f}km/h | 位置：({vehicle_loc.x:.1f}, {vehicle_loc.y:.1f}) | 转向：{steer:.2f}", end='\r')
+
+ except Exception as e:
+ print(f"\n程序异常：{e}")
+ finally:
+ # 清理资源
+ print("\n清理资源中...")
+ settings = world.get_settings()
+ settings.synchronous_mode = False
+ world.apply_settings(settings)
+ if 'vehicle' in locals():
+ vehicle.destroy()
+ if 'camera' in locals():
+ camera.destroy()
+ cv2.destroyAllWindows()
+ plt.close('all')
+ time.sleep(1)
+ print("仿真结束")
+
+if __name__ == '__main__':
main()
\ No newline at end of file
From 9c79eecc021b0aa835c537580c2789d136411399 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sat, 20 Dec 2025 22:50:45 +0800
Subject: [PATCH 12/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=81=BF=E9=9A=9C?=
=?UTF-8?q?=E6=8E=A7=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 798 ++++++++++++++++++-----------
1 file changed, 488 insertions(+), 310 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 2e6e2fd2f2..422e90076b 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,341 +1,519 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-"""
-CARLA Waypoint Following（原生道路航点版）
-核心：直接使用CARLA地图的道路航点，车辆100%沿道路行驶
-"""
-
-import sys
-import os
import carla
+import time
import numpy as np
-import math
-import matplotlib.pyplot as plt
import cv2
-import time
+import math
+from collections import deque
+import random
+import os
-# ===================== 核心配置 =====================
-class Config:
- # 速度控制
- TARGET_SPEED = 20.0 # km/h
- PID_KP = 0.3
- PID_KI = 0.02
- PID_KD = 0.01
-
- # 纯追踪算法参数
- LOOKAHEAD_DISTANCE = 8.0 # 前瞻距离（米）
- MAX_STEER_ANGLE = 30.0 # 最大转向角（度）
-
- # 摄像头配置
- CAMERA_WIDTH = 800
- CAMERA_HEIGHT = 600
- CAMERA_FOV = 90
-
- # 车辆配置
- VEHICLE_MODEL = "vehicle.tesla.model3"
-
- # 可视化配置
- PLOT_SIZE = (12, 10)
- WAYPOINT_COUNT = 50 # 预先生成的道路航点数量
-
-# ===================== 工具类 =====================
-class Tools:
- @staticmethod
- def normalize_angle(angle):
- """将角度归一化到[-pi, pi]"""
- while angle > np.pi:
- angle -= 2 * np.pi
- while angle < -np.pi:
- angle += 2 * np.pi
- return angle
-
- @staticmethod
- def get_vehicle_pose(vehicle):
- """获取车辆的位置、朝向和速度"""
+# --------------------------
+# 1. 障碍物检测器（优化性能+无延迟检测）
+# --------------------------
+class ObstacleDetector:
+ def __init__(self, world, vehicle, max_distance=50.0, detect_interval=1): # 改为1帧检测一次
+ self.world = world
+ self.vehicle = vehicle
+ self.max_distance = max_distance
+ self.detect_interval = detect_interval # 检测间隔（帧）
+ self.frame_count = 0
+ self.last_obstacle_info = {
+ 'has_obstacle': False,
+ 'distance': float('inf'),
+ 'relative_angle': 0.0,
+ 'obstacle_type': None,
+ 'obstacle_speed': 0.0,
+ 'relative_speed': 0.0 # 新增：自车与障碍物的相对速度
+ }
+
+ def get_vehicle_speed(self, vehicle):
+ """获取车辆速度（km/h）"""
+ velocity = vehicle.get_velocity()
+ speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)
+ return speed * 3.6
+
+ def get_obstacle_info(self):
+ """检测前方障碍物信息（无延迟检测）"""
+ self.frame_count += 1
+ if self.frame_count % self.detect_interval != 0:
+ return self.last_obstacle_info
+
+ try:
+ vehicle_transform = self.vehicle.get_transform()
+ vehicle_location = vehicle_transform.location
+ forward_vector = vehicle_transform.get_forward_vector()
+ self_speed = self.get_vehicle_speed(self.vehicle) # 自车速度
+
+ # 减少get_actors调用频率，只获取车辆
+ all_vehicles = self.world.get_actors().filter('vehicle.*')
+ min_distance = float('inf')
+ closest_obstacle = None
+ relative_angle = 0.0
+ obstacle_speed = 0.0
+
+ for other_vehicle in all_vehicles:
+ if other_vehicle.id == self.vehicle.id:
+ continue
+
+ other_location = other_vehicle.get_location()
+ distance = vehicle_location.distance(other_location)
+ if distance > self.max_distance:
+ continue
+
+ # 计算相对角度（仅前方±70度，扩大检测范围）
+ relative_vector = carla.Location(
+ other_location.x - vehicle_location.x,
+ other_location.y - vehicle_location.y,
+ 0
+ )
+ forward_2d = carla.Vector3D(forward_vector.x, forward_vector.y, 0)
+ relative_2d = carla.Vector3D(relative_vector.x, relative_vector.y, 0)
+
+ # 向量归一化
+ forward_norm = math.sqrt(forward_2d.x ** 2 + forward_2d.y ** 2)
+ relative_norm = math.sqrt(relative_2d.x ** 2 + relative_2d.y ** 2)
+ if forward_norm == 0 or relative_norm == 0:
+ continue
+
+ dot_product = forward_2d.x * relative_2d.x + forward_2d.y * relative_2d.y
+ cos_angle = dot_product / (forward_norm * relative_norm)
+ cos_angle = max(-1.0, min(1.0, cos_angle))
+ angle_deg = math.degrees(math.acos(cos_angle))
+
+ # 扩大检测角度到±70度，更早发现障碍物
+ if angle_deg <= 70 and distance < min_distance:
+ min_distance = distance
+ closest_obstacle = other_vehicle
+ obstacle_speed = self.get_vehicle_speed(other_vehicle)
+ # 确定角度方向
+ relative_angle = angle_deg if relative_2d.y >= 0 else -angle_deg
+
+ # 计算相对速度（自车速度 - 前车速度，正数表示自车更快）
+ relative_speed = self_speed - obstacle_speed if closest_obstacle else 0.0
+
+ # 更新障碍物信息
+ if closest_obstacle is not None:
+ self.last_obstacle_info = {
+ 'has_obstacle': True,
+ 'distance': min_distance,
+ 'relative_angle': relative_angle,
+ 'obstacle_type': closest_obstacle.type_id,
+ 'obstacle_speed': obstacle_speed,
+ 'relative_speed': relative_speed
+ }
+ else:
+ self.last_obstacle_info = {
+ 'has_obstacle': False,
+ 'distance': float('inf'),
+ 'relative_angle': 0.0,
+ 'obstacle_type': None,
+ 'obstacle_speed': 0.0,
+ 'relative_speed': 0.0
+ }
+
+ except Exception as e:
+ print(f"障碍物检测错误: {e}")
+
+ return self.last_obstacle_info
+
+ def visualize_obstacles(self, image, vehicle_transform):
+ """在图像上可视化障碍物检测结果"""
+ if not self.last_obstacle_info['has_obstacle']:
+ return image
+
+ height, width = image.shape[:2]
+ distance = self.last_obstacle_info['distance']
+ angle = self.last_obstacle_info['relative_angle']
+
+ # 计算障碍物在图像中的位置
+ x_pos = int(width / 2 + (angle / 70) * (width / 2)) # 适配70度检测范围
+ x_pos = max(0, min(width - 1, x_pos))
+
+ # 根据距离设置颜色和大小
+ if distance < 15:
+ color = (0, 0, 255)
+ radius = 15
+ elif distance < 30:
+ color = (0, 165, 255)
+ radius = 10
+ else:
+ color = (0, 255, 255)
+ radius = 5
+
+ # 绘制障碍物指示器
+ cv2.circle(image, (x_pos, int(height * 0.8)), radius, color, -1)
+ cv2.putText(image, f"{distance:.1f}m", (x_pos - 20, int(height * 0.8) - 20),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
+ # 绘制相对速度
+ rel_speed = self.last_obstacle_info['relative_speed']
+ cv2.putText(image, f"RelSpeed: {rel_speed:.1f}km/h", (x_pos - 20, int(height * 0.8) + 20),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
+ return image
+
+# --------------------------
+# 2. 传统控制器（核心控制逻辑+优化避障）
+# --------------------------
+class TraditionalController:
+ """基于路点的传统控制器，整合避障"""
+ def __init__(self, world, obstacle_detector):
+ self.world = world
+ self.map = world.get_map()
+ self.waypoint_distance = 10.0
+ self.obstacle_detector = obstacle_detector
+ # 调整避障阈值：扩大距离，提前减速
+ self.emergency_brake_distance = 12.0 # 从6米改为12米
+ self.safe_following_distance = 18.0 # 从10米改为18米
+ self.early_warning_distance = 30.0 # 新增：30米提前预警
+
+ def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info):
+ """传统控制器的避障逻辑（优化刹车力度+相对速度）"""
+ if not obstacle_info['has_obstacle']:
+ return throttle, brake, steer
+
+ distance = obstacle_info['distance']
+ angle = obstacle_info['relative_angle']
+ vehicle_speed = self.obstacle_detector.get_vehicle_speed(vehicle)
+ relative_speed = obstacle_info['relative_speed'] # 自车与前车的相对速度
+
+ # 1. 提前预警（30米内）：轻微减速，降低油门
+ if distance < self.early_warning_distance and relative_speed > 0:
+ throttle *= 0.5 # 油门减半
+ if vehicle_speed > 30:
+ brake = 0.2 # 轻微刹车
+
+ # 2. 紧急刹车（12米内）：全力刹车+手刹
+ if distance < self.emergency_brake_distance:
+ print(f"紧急刹车！距离前车: {distance:.1f}m, 相对速度: {relative_speed:.1f}km/h")
+ return 0.0, 1.0, 0.0 # brake=1.0 + 后续拉手刹
+
+ # 3. 安全跟车（18米内）：动态调整刹车力度
+ elif distance < self.safe_following_distance:
+ # 根据距离和相对速度计算所需刹车力度
+ required_distance = max(8.0, vehicle_speed * 0.5) # 增加安全车距系数
+ distance_ratio = (distance - required_distance) / self.safe_following_distance
+ distance_ratio = max(0.0, min(1.0, distance_ratio))
+
+ # 相对速度越大，刹车越重
+ brake_strength = (1 - distance_ratio) * 0.8 + (relative_speed / 20) * 0.2
+ brake_strength = max(0.3, min(0.8, brake_strength))
+
+ if distance < required_distance:
+ throttle = 0.0
+ brake = brake_strength
+ else:
+ throttle = 0.1
+ brake = 0.0
+
+ # 尝试变道
+ if abs(angle) < 15:
+ location = vehicle.get_location()
+ waypoint = self.map.get_waypoint(location)
+ left_lane = waypoint.get_left_lane()
+ right_lane = waypoint.get_right_lane()
+
+ if left_lane and left_lane.lane_type == carla.LaneType.Driving:
+ steer = -0.3
+ elif right_lane and right_lane.lane_type == carla.LaneType.Driving:
+ steer = 0.3
+ else:
+ steer = 0.2 if angle >= 0 else -0.2
+
+ return throttle, brake, steer
+
+ def get_control(self, vehicle):
+ """生成传统控制指令（优先避障，弱化基础速度控制）"""
+ # 获取车辆状态
transform = vehicle.get_transform()
- loc = transform.location
- yaw = math.radians(transform.rotation.yaw)
- vel = vehicle.get_velocity()
- speed = 3.6 * np.linalg.norm([vel.x, vel.y, vel.z])
- return loc, yaw, speed
-
- @staticmethod
- def clear_all_actors(world):
- """清理所有车辆和传感器"""
- for actor in world.get_actors():
- try:
- if actor.type_id.startswith('vehicle') or actor.type_id.startswith('sensor'):
- actor.destroy()
- except:
- pass
- time.sleep(0.5)
- print("已清理所有残留Actor")
-
- @staticmethod
- def focus_vehicle(world, vehicle):
- """将CARLA客户端视角聚焦到车辆"""
- spectator = world.get_spectator()
- t = vehicle.get_transform()
- spectator.set_transform(carla.Transform(t.location + carla.Location(x=0, y=-8, z=5), t.rotation))
- print("CARLA客户端已聚焦到车辆")
-
- @staticmethod
- def generate_road_waypoints(world, start_loc, count=50, step=2.0):
- """
- 从起点沿道路生成连续的原生航点
- :param world: CARLA世界对象
- :param start_loc: 起点位置
- :param count: 航点数量
- :param step: 每个航点的步长（米）
- :return: 航点列表[(x, y, z), ...]
- """
- waypoints = []
- map = world.get_map()
- wp = map.get_waypoint(start_loc)
- for i in range(count):
- waypoints.append((wp.transform.location.x, wp.transform.location.y, wp.transform.location.z))
- # 沿道路下一个航点（直走，不考虑分叉）
- wp = wp.next(step)[0]
- print(f"生成了{len(waypoints)}个原生道路航点")
- return waypoints
-
-# ===================== 摄像头回调 =====================
-def camera_callback(image, data_dict):
- array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))[:, :, :3]
- data_dict['image'] = array
-
-# ===================== 控制器 =====================
-class PIDSpeedController:
- def __init__(self, config):
- self.kp = config.PID_KP
- self.ki = config.PID_KI
- self.kd = config.PID_KD
- self.prev_err = 0.0
- self.integral = 0.0
- self.target_speed = config.TARGET_SPEED
-
- def calculate(self, current_speed):
- err = self.target_speed - current_speed
- self.integral = np.clip(self.integral + err * 0.05, -1.0, 1.0)
- deriv = (err - self.prev_err) / 0.05
- output = self.kp * err + self.ki * self.integral + self.kd * deriv
- self.prev_err = err
- return np.clip(output, 0.1, 1.0)
-
-class PurePursuitController:
- def __init__(self, config):
- self.lookahead_dist = config.LOOKAHEAD_DISTANCE
- self.max_steer_rad = math.radians(config.MAX_STEER_ANGLE)
-
- def calculate_steer(self, vehicle_loc, vehicle_yaw, waypoints):
- """
- 纯追踪算法计算转向角
- :param vehicle_loc: 车辆位置
- :param vehicle_yaw: 车辆朝向（弧度）
- :param waypoints: 道路航点列表
- :return: 转向角（-1~1）
- """
- # 1. 将航点转换为车辆坐标系
- wp_coords = np.array(waypoints)
- vehicle_x = vehicle_loc.x
- vehicle_y = vehicle_loc.y
-
- # 旋转和平移（车辆坐标系：x向前，y向左）
- cos_yaw = math.cos(vehicle_yaw)
- sin_yaw = math.sin(vehicle_yaw)
- translated_x = wp_coords[:, 0] - vehicle_x
- translated_y = wp_coords[:, 1] - vehicle_y
- rotated_x = translated_x * cos_yaw + translated_y * sin_yaw
- rotated_y = -translated_x * sin_yaw + translated_y * cos_yaw
-
- # 2. 找到距离车辆>=前瞻距离的第一个航点
- distances = np.hypot(rotated_x, rotated_y)
- valid_wp_indices = np.where(distances >= self.lookahead_dist)[0]
- if len(valid_wp_indices) == 0:
- return 0.0
-
- target_idx = valid_wp_indices[0]
- target_x = rotated_x[target_idx]
- target_y = rotated_y[target_idx]
-
- # 3. 计算转向角（纯追踪公式：steer = arctan(2*L*y/(x²+y²))，L为车辆轴距，这里简化为1.0）
- L = 1.0 # 车辆轴距（米）
- steer_rad = math.atan2(2 * L * target_y, self.lookahead_dist ** 2)
-
- # 4. 限制转向角
- steer_rad = np.clip(steer_rad, -self.max_steer_rad, self.max_steer_rad)
- steer = steer_rad / self.max_steer_rad
-
- return steer
-
-# ===================== 可视化 =====================
-class Visualizer:
- def __init__(self, config, spawn_loc, initial_waypoints):
- self.waypoints = np.array(initial_waypoints)
- self.trajectory = []
- self.spawn_loc = (spawn_loc.x, spawn_loc.y)
-
- plt.rcParams['backend'] = 'TkAgg'
- plt.ioff()
- self.fig, self.ax = plt.subplots(figsize=config.PLOT_SIZE)
-
- # 绘制道路航点（CARLA原生）
- self.ax.scatter(self.waypoints[:, 0], self.waypoints[:, 1], c='blue', s=50, label='Road Waypoints (CARLA)', zorder=3)
- # 绘制生成点
- self.ax.scatter(self.spawn_loc[0], self.spawn_loc[1], c='orange', marker='s', s=150, label='Spawn Point', zorder=5)
- # 轨迹和车辆
- self.traj_line, = self.ax.plot([], [], c='red', linewidth=4, label='Vehicle Trajectory', zorder=2)
- self.vehicle_dot, = self.ax.plot([], [], c='green', marker='o', markersize=20, label='Vehicle', zorder=6)
-
- self.ax.set_xlabel('X (m)', fontsize=14)
- self.ax.set_ylabel('Y (m)', fontsize=14)
- self.ax.set_title('CARLA Road Following (Native Waypoints)', fontsize=16)
- self.ax.legend(fontsize=12)
- self.ax.grid(True, alpha=0.3)
- self.ax.axis('equal')
-
- plt.show(block=False)
- self.fig.canvas.draw()
- self.fig.canvas.flush_events()
-
- def update(self, vehicle_x, vehicle_y, new_waypoints=None):
- """更新轨迹和航点"""
- self.trajectory.append([vehicle_x, vehicle_y])
- if len(self.trajectory) > 1000:
- self.trajectory = self.trajectory[-1000:]
-
- # 更新轨迹
- traj = np.array(self.trajectory)
- self.traj_line.set_data(traj[:, 0], traj[:, 1])
- self.vehicle_dot.set_data(vehicle_x, vehicle_y)
-
- # 更新航点（如果有新航点）
- if new_waypoints is not None:
- self.waypoints = np.array(new_waypoints)
- self.ax.scatter(self.waypoints[:, 0], self.waypoints[:, 1], c='blue', s=50, zorder=3)
-
- self.ax.relim()
- self.ax.autoscale_view(True, True, True)
- self.fig.canvas.draw()
- self.fig.canvas.flush_events()
-
-# ===================== 主函数 =====================
+ location = vehicle.get_location()
+ velocity = vehicle.get_velocity()
+ speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6
+
+ # 获取障碍物信息
+ obstacle_info = self.obstacle_detector.get_obstacle_info()
+
+ # 获取路点
+ waypoint = self.map.get_waypoint(location, project_to_road=True)
+ next_waypoints = waypoint.next(self.waypoint_distance)
+ target_waypoint = next_waypoints[0] if next_waypoints else waypoint
+
+ # 计算转向
+ vehicle_yaw = math.radians(transform.rotation.yaw)
+ target_loc = target_waypoint.transform.location
+
+ dx = target_loc.x - location.x
+ dy = target_loc.y - location.y
+
+ local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw)
+ local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw)
+
+ if abs(local_x) < 0.1:
+ steer = 0.0
+ else:
+ angle = math.atan2(local_y, local_x)
+ steer = np.clip(angle / math.radians(45), -1.0, 1.0)
+
+ # 基础速度控制（降低优先级）
+ if speed < 20:
+ throttle = 0.4 # 从0.6降低到0.4，减少油门
+ brake = 0.0
+ elif speed < 40:
+ throttle = 0.2 # 从0.4降低到0.2
+ brake = 0.0
+ else:
+ throttle = 0.1
+ brake = 0.2
+
+ # 障碍物调整（优先执行）
+ throttle, brake, steer = self.apply_obstacle_avoidance(throttle, brake, steer, vehicle, obstacle_info)
+
+ # 低速强油门（仅当无障碍物时生效）
+ if speed < 5.0 and not obstacle_info['has_obstacle']:
+ throttle = 0.4
+ brake = 0.0
+
+ return throttle, brake, steer
+
+# --------------------------
+# 3. CARLA环境初始化（适配0.9.10）
+# --------------------------
def main():
- config = Config()
- tools = Tools()
-
- # 初始化OpenCV窗口
- cv2.namedWindow('CARLA Vehicle View', cv2.WINDOW_NORMAL)
- cv2.resizeWindow('CARLA Vehicle View', config.CAMERA_WIDTH, config.CAMERA_HEIGHT)
-
- # 连接CARLA
try:
+ # 连接CARLA服务器（0.9.10兼容）
client = carla.Client('localhost', 2000)
- client.set_timeout(30.0)
- world = client.load_world('Town03')
- map = world.get_map()
+ client.set_timeout(15.0)
+ world = client.load_world('Town01') # 0.9.10支持Town01
- # 设置同步模式
+ # 设置同步模式（0.9.10关键配置）
settings = world.get_settings()
settings.synchronous_mode = True
- settings.fixed_delta_seconds = 0.05
+ settings.fixed_delta_seconds = 0.1
world.apply_settings(settings)
- # 清理Actor
- tools.clear_all_actors(world)
+ # 设置天气
+ weather = carla.WeatherParameters(
+ cloudiness=30.0,
+ precipitation=0.0,
+ sun_altitude_angle=70.0
+ )
+ world.set_weather(weather)
- # 获取道路生成点
+ # 获取出生点
+ map = world.get_map()
spawn_points = map.get_spawn_points()
- spawn_transform = spawn_points[0]
- print(f"使用道路生成点：{spawn_transform.location}")
-
- # 生成车辆
- bp_lib = world.get_blueprint_library()
- vehicle_bp = bp_lib.find(config.VEHICLE_MODEL)
- vehicle = world.spawn_actor(vehicle_bp, spawn_transform)
+ if not spawn_points:
+ raise Exception("无可用出生点")
+ spawn_point = spawn_points[10]
+
+ # 生成主车辆
+ blueprint_library = world.get_blueprint_library()
+ vehicle_bp = blueprint_library.find('vehicle.tesla.model3')
+ vehicle_bp.set_attribute('color', '255,0,0')
+ vehicle = world.spawn_actor(vehicle_bp, spawn_point)
if not vehicle:
- print("车辆生成失败！")
- return
- print(f"车辆{config.VEHICLE_MODEL}生成成功")
-
- # 聚焦车辆
- tools.focus_vehicle(world, vehicle)
-
- # 挂载摄像头
- camera_bp = bp_lib.find('sensor.camera.rgb')
- camera_bp.set_attribute('image_size_x', str(config.CAMERA_WIDTH))
- camera_bp.set_attribute('image_size_y', str(config.CAMERA_HEIGHT))
- camera_bp.set_attribute('fov', str(config.CAMERA_FOV))
- camera = world.spawn_actor(camera_bp, carla.Transform(carla.Location(x=2.0, z=1.8)), attach_to=vehicle)
-
- # 摄像头数据
- camera_data = {'image': np.zeros((config.CAMERA_HEIGHT, config.CAMERA_WIDTH, 3), dtype=np.uint8)}
- camera.listen(lambda img: camera_callback(img, camera_data))
-
- # 生成初始道路航点
- initial_waypoints = tools.generate_road_waypoints(world, spawn_transform.location, config.WAYPOINT_COUNT)
-
- # 初始化控制器
- speed_controller = PIDSpeedController(config)
- path_controller = PurePursuitController(config)
-
- # 初始化可视化
- visualizer = Visualizer(config, spawn_transform.location, initial_waypoints)
+ raise Exception("无法生成主车辆")
+ vehicle.set_autopilot(False)
+ print(f"车辆生成在位置: {spawn_point.location}")
+
+ # 生成障碍物车辆（调整生成位置，确保在前车前方）
+ obstacle_count = 3
+ for i in range(obstacle_count):
+ spawn_idx = (i + 12) % len(spawn_points) # 从15改为12，更靠近主车辆
+ other_vehicle_bp = random.choice(blueprint_library.filter('vehicle.*'))
+ other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx])
+ if other_vehicle:
+ other_vehicle.set_autopilot(True)
+
+ # 配置传感器（0.9.10兼容）
+ # 后视角相机
+ third_camera_bp = blueprint_library.find('sensor.camera.rgb')
+ third_camera_bp.set_attribute('image_size_x', '640')
+ third_camera_bp.set_attribute('image_size_y', '480')
+ third_camera_bp.set_attribute('fov', '110')
+ third_camera_transform = carla.Transform(
+ carla.Location(x=-5.0, y=0.0, z=3.0),
+ carla.Rotation(pitch=-15.0)
+ )
+ third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle)
+
+ # 前视角相机
+ front_camera_bp = blueprint_library.find('sensor.camera.rgb')
+ front_camera_bp.set_attribute('image_size_x', '640')
+ front_camera_bp.set_attribute('image_size_y', '480')
+ front_camera_bp.set_attribute('fov', '90')
+ front_camera_transform = carla.Transform(
+ carla.Location(x=2.0, y=0.0, z=1.5),
+ carla.Rotation(pitch=0.0)
+ )
+ front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle)
+
+ # 传感器数据存储
+ third_image = None
+ front_image = None
+
+ # 传感器回调函数
+ def third_camera_callback(image):
+ nonlocal third_image
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = np.reshape(array, (image.height, image.width, 4))
+ third_image = array[:, :, :3]
+
+ def front_camera_callback(image):
+ nonlocal front_image
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = np.reshape(array, (image.height, image.width, 4))
+ front_image = array[:, :, :3]
+
+ # 注册回调
+ third_camera.listen(third_camera_callback)
+ front_camera.listen(front_camera_callback)
+ time.sleep(2.0) # 等待传感器初始化
+
+ # 初始化核心组件
+ obstacle_detector = ObstacleDetector(world, vehicle)
+ traditional_controller = TraditionalController(world, obstacle_detector)
+
+ # 控制变量
+ throttle = 0.3
+ steer = 0.0
+ brake = 0.0
+ frame_count = 0
+ stuck_count = 0
+ last_position = vehicle.get_location()
# 主循环
+ print("自动驾驶系统启动 - 仅使用传统控制器")
+ print("控制键: q-退出, r-重置车辆")
+
while True:
world.tick()
+ frame_count += 1
# 获取车辆状态
- vehicle_loc, vehicle_yaw, current_speed = tools.get_vehicle_pose(vehicle)
-
- # 实时更新道路航点（每10帧更新一次，减少计算量）
- if world.get_snapshot().frame % 10 == 0:
- new_waypoints = tools.generate_road_waypoints(world, vehicle_loc, config.WAYPOINT_COUNT)
+ vehicle_transform = vehicle.get_transform()
+ vehicle_location = vehicle.get_transform().location
+ vehicle_velocity = vehicle.get_velocity()
+ vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2)
+
+ # 检测障碍物
+ obstacle_info = obstacle_detector.get_obstacle_info()
+
+ # 卡住检测（优化：有障碍物时不触发）
+ distance_moved = vehicle_location.distance(last_position)
+ is_moving = distance_moved > 0.2 or vehicle_speed > 1.0
+ # 关键修正：通过traditional_controller实例访问属性，而非self
+ if obstacle_info['has_obstacle'] and obstacle_info['distance'] < traditional_controller.safe_following_distance:
+ stuck_count = 0
else:
- new_waypoints = None
-
- # 更新可视化
- visualizer.update(vehicle_loc.x, vehicle_loc.y, new_waypoints)
-
- # 显示摄像头画面
- cv2.imshow('CARLA Vehicle View', camera_data['image'])
- if cv2.waitKey(1) & 0xFF == ord('q'):
- break
-
- # 计算控制量
- steer = path_controller.calculate_steer(vehicle_loc, vehicle_yaw, initial_waypoints)
- throttle = speed_controller.calculate(current_speed)
- brake = 1.0 if current_speed > config.TARGET_SPEED * 2 else 0.0
-
- # 控制车辆
- vehicle.apply_control(carla.VehicleControl(
- throttle=throttle,
- brake=brake,
- steer=steer,
- hand_brake=False,
- reverse=False
- ))
-
- # 打印状态
- print(f"速度：{current_speed:.1f}km/h | 位置：({vehicle_loc.x:.1f}, {vehicle_loc.y:.1f}) | 转向：{steer:.2f}", end='\r')
+ if not is_moving:
+ stuck_count += 1
+ else:
+ stuck_count = 0
+ last_position = vehicle_location
+
+ # 卡住恢复（仅当无障碍物时执行）
+ if stuck_count > 20: # 从15帧改为20帧，降低误触发
+ print("检测到车辆卡住，执行恢复程序...")
+ # 紧急刹车
+ vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0, brake=1.0, hand_brake=True))
+ time.sleep(0.5)
+ # 倒车或转向
+ if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 15:
+ vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0, reverse=True))
+ time.sleep(1.0)
+ else:
+ recovery_steer = random.choice([-0.5, 0.5])
+ vehicle.apply_control(carla.VehicleControl(throttle=0.6, steer=recovery_steer, brake=0.0)) # 降低油门到0.6
+ time.sleep(1.0)
+ stuck_count = 0
+
+ # 生成控制指令（仅使用传统控制器）
+ throttle, brake, steer = traditional_controller.get_control(vehicle)
+
+ # 紧急刹车时拉手刹
+ if brake >= 1.0:
+ vehicle.apply_control(carla.VehicleControl(
+ throttle=0.0, steer=steer, brake=1.0, hand_brake=True
+ ))
+ else:
+ # 应用控制
+ control = carla.VehicleControl(
+ throttle=throttle,
+ steer=steer,
+ brake=brake,
+ hand_brake=False,
+ reverse=False
+ )
+ vehicle.apply_control(control)
+
+ # 图像显示
+ if third_image is not None:
+ display_image = third_image.copy()
+ # 可视化障碍物
+ display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform)
+ # 绘制信息
+ cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+ cv2.putText(display_image, f"Mode: Traditional", (10, 60),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+ cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+ cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+ cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+ # 障碍物信息
+ if obstacle_info['has_obstacle']:
+ cv2.putText(display_image, f"Obstacle: {obstacle_info['distance']:.1f}m", (10, 180),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+ cv2.putText(display_image, f"RelSpeed: {obstacle_info['relative_speed']:.1f}km/h", (10, 210),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+ else:
+ cv2.putText(display_image, "Obstacle: None", (10, 180),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+ # 卡住警告
+ if stuck_count > 5:
+ cv2.putText(display_image, "STUCK DETECTED!", (10, 240),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
+
+ cv2.imshow('CARLA Autopilot (0.9.10)', display_image)
+
+ # 键盘控制
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord('q'):
+ break
+ elif key == ord('r'):
+ vehicle.set_transform(spawn_point)
+ throttle = 0.3
+ steer = 0.0
+ brake = 0.0
+ stuck_count = 0
+ print("车辆已重置")
+
+ time.sleep(0.01)
except Exception as e:
- print(f"\n程序异常：{e}")
+ print(f"系统错误: {e}")
+ import traceback
+ traceback.print_exc()
+
finally:
- # 清理资源
- print("\n清理资源中...")
+ # 清理资源（0.9.10兼容）
+ print("正在清理资源...")
+ cv2.destroyAllWindows()
+ # 停止传感器
+ if 'third_camera' in locals():
+ third_camera.stop()
+ if 'front_camera' in locals():
+ front_camera.stop()
+ # 销毁所有Actor
+ for actor in world.get_actors():
+ if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'):
+ actor.destroy()
+ # 关闭同步模式
settings = world.get_settings()
settings.synchronous_mode = False
world.apply_settings(settings)
- if 'vehicle' in locals():
- vehicle.destroy()
- if 'camera' in locals():
- camera.destroy()
- cv2.destroyAllWindows()
- plt.close('all')
- time.sleep(1)
- print("仿真结束")
+ print("资源清理完成")
-if __name__ == '__main__':
+if __name__ == "__main__":
main()
\ No newline at end of file
From 3286110cc839d4b23403a88236ea4f1d28deb5d2 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sun, 21 Dec 2025 11:40:45 +0800
Subject: [PATCH 13/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E4=BA=A4?=
=?UTF-8?q?=E9=80=9A=E4=BF=A1=E5=8F=B7=E7=81=AF=E5=93=8D=E5=BA=94?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 1051 +++++++++++++++-------------
1 file changed, 566 insertions(+), 485 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 422e90076b..e86acaf6c1 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,519 +1,600 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+CARLA 多交通灯版：Town04密集交通灯+状态循环+车辆响应
+"""
+
+import sys
+import os
import carla
-import time
import numpy as np
-import cv2
import math
-from collections import deque
-import random
-import os
-
-# --------------------------
-# 1. 障碍物检测器（优化性能+无延迟检测）
-# --------------------------
-class ObstacleDetector:
- def __init__(self, world, vehicle, max_distance=50.0, detect_interval=1): # 改为1帧检测一次
- self.world = world
- self.vehicle = vehicle
- self.max_distance = max_distance
- self.detect_interval = detect_interval # 检测间隔（帧）
- self.frame_count = 0
- self.last_obstacle_info = {
- 'has_obstacle': False,
- 'distance': float('inf'),
- 'relative_angle': 0.0,
- 'obstacle_type': None,
- 'obstacle_speed': 0.0,
- 'relative_speed': 0.0 # 新增：自车与障碍物的相对速度
- }
-
- def get_vehicle_speed(self, vehicle):
- """获取车辆速度（km/h）"""
- velocity = vehicle.get_velocity()
- speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)
- return speed * 3.6
-
- def get_obstacle_info(self):
- """检测前方障碍物信息（无延迟检测）"""
- self.frame_count += 1
- if self.frame_count % self.detect_interval != 0:
- return self.last_obstacle_info
+import pygame
+import traceback
+import time
- try:
- vehicle_transform = self.vehicle.get_transform()
- vehicle_location = vehicle_transform.location
- forward_vector = vehicle_transform.get_forward_vector()
- self_speed = self.get_vehicle_speed(self.vehicle) # 自车速度
-
- # 减少get_actors调用频率，只获取车辆
- all_vehicles = self.world.get_actors().filter('vehicle.*')
- min_distance = float('inf')
- closest_obstacle = None
- relative_angle = 0.0
- obstacle_speed = 0.0
-
- for other_vehicle in all_vehicles:
- if other_vehicle.id == self.vehicle.id:
- continue
-
- other_location = other_vehicle.get_location()
- distance = vehicle_location.distance(other_location)
- if distance > self.max_distance:
- continue
-
- # 计算相对角度（仅前方±70度，扩大检测范围）
- relative_vector = carla.Location(
- other_location.x - vehicle_location.x,
- other_location.y - vehicle_location.y,
- 0
- )
- forward_2d = carla.Vector3D(forward_vector.x, forward_vector.y, 0)
- relative_2d = carla.Vector3D(relative_vector.x, relative_vector.y, 0)
-
- # 向量归一化
- forward_norm = math.sqrt(forward_2d.x ** 2 + forward_2d.y ** 2)
- relative_norm = math.sqrt(relative_2d.x ** 2 + relative_2d.y ** 2)
- if forward_norm == 0 or relative_norm == 0:
- continue
-
- dot_product = forward_2d.x * relative_2d.x + forward_2d.y * relative_2d.y
- cos_angle = dot_product / (forward_norm * relative_norm)
- cos_angle = max(-1.0, min(1.0, cos_angle))
- angle_deg = math.degrees(math.acos(cos_angle))
-
- # 扩大检测角度到±70度，更早发现障碍物
- if angle_deg <= 70 and distance < min_distance:
- min_distance = distance
- closest_obstacle = other_vehicle
- obstacle_speed = self.get_vehicle_speed(other_vehicle)
- # 确定角度方向
- relative_angle = angle_deg if relative_2d.y >= 0 else -angle_deg
-
- # 计算相对速度（自车速度 - 前车速度，正数表示自车更快）
- relative_speed = self_speed - obstacle_speed if closest_obstacle else 0.0
-
- # 更新障碍物信息
- if closest_obstacle is not None:
- self.last_obstacle_info = {
- 'has_obstacle': True,
- 'distance': min_distance,
- 'relative_angle': relative_angle,
- 'obstacle_type': closest_obstacle.type_id,
- 'obstacle_speed': obstacle_speed,
- 'relative_speed': relative_speed
- }
- else:
- self.last_obstacle_info = {
- 'has_obstacle': False,
- 'distance': float('inf'),
- 'relative_angle': 0.0,
- 'obstacle_type': None,
- 'obstacle_speed': 0.0,
- 'relative_speed': 0.0
- }
-
- except Exception as e:
- print(f"障碍物检测错误: {e}")
-
- return self.last_obstacle_info
-
- def visualize_obstacles(self, image, vehicle_transform):
- """在图像上可视化障碍物检测结果"""
- if not self.last_obstacle_info['has_obstacle']:
- return image
-
- height, width = image.shape[:2]
- distance = self.last_obstacle_info['distance']
- angle = self.last_obstacle_info['relative_angle']
-
- # 计算障碍物在图像中的位置
- x_pos = int(width / 2 + (angle / 70) * (width / 2)) # 适配70度检测范围
- x_pos = max(0, min(width - 1, x_pos))
-
- # 根据距离设置颜色和大小
- if distance < 15:
- color = (0, 0, 255)
- radius = 15
- elif distance < 30:
- color = (0, 165, 255)
- radius = 10
- else:
- color = (0, 255, 255)
- radius = 5
-
- # 绘制障碍物指示器
- cv2.circle(image, (x_pos, int(height * 0.8)), radius, color, -1)
- cv2.putText(image, f"{distance:.1f}m", (x_pos - 20, int(height * 0.8) - 20),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
- # 绘制相对速度
- rel_speed = self.last_obstacle_info['relative_speed']
- cv2.putText(image, f"RelSpeed: {rel_speed:.1f}km/h", (x_pos - 20, int(height * 0.8) + 20),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
- return image
-
-# --------------------------
-# 2. 传统控制器（核心控制逻辑+优化避障）
-# --------------------------
-class TraditionalController:
- """基于路点的传统控制器，整合避障"""
- def __init__(self, world, obstacle_detector):
- self.world = world
- self.map = world.get_map()
- self.waypoint_distance = 10.0
- self.obstacle_detector = obstacle_detector
- # 调整避障阈值：扩大距离，提前减速
- self.emergency_brake_distance = 12.0 # 从6米改为12米
- self.safe_following_distance = 18.0 # 从10米改为18米
- self.early_warning_distance = 30.0 # 新增：30米提前预警
-
- def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info):
- """传统控制器的避障逻辑（优化刹车力度+相对速度）"""
- if not obstacle_info['has_obstacle']:
- return throttle, brake, steer
-
- distance = obstacle_info['distance']
- angle = obstacle_info['relative_angle']
- vehicle_speed = self.obstacle_detector.get_vehicle_speed(vehicle)
- relative_speed = obstacle_info['relative_speed'] # 自车与前车的相对速度
-
- # 1. 提前预警（30米内）：轻微减速，降低油门
- if distance < self.early_warning_distance and relative_speed > 0:
- throttle *= 0.5 # 油门减半
- if vehicle_speed > 30:
- brake = 0.2 # 轻微刹车
-
- # 2. 紧急刹车（12米内）：全力刹车+手刹
- if distance < self.emergency_brake_distance:
- print(f"紧急刹车！距离前车: {distance:.1f}m, 相对速度: {relative_speed:.1f}km/h")
- return 0.0, 1.0, 0.0 # brake=1.0 + 后续拉手刹
-
- # 3. 安全跟车（18米内）：动态调整刹车力度
- elif distance < self.safe_following_distance:
- # 根据距离和相对速度计算所需刹车力度
- required_distance = max(8.0, vehicle_speed * 0.5) # 增加安全车距系数
- distance_ratio = (distance - required_distance) / self.safe_following_distance
- distance_ratio = max(0.0, min(1.0, distance_ratio))
-
- # 相对速度越大，刹车越重
- brake_strength = (1 - distance_ratio) * 0.8 + (relative_speed / 20) * 0.2
- brake_strength = max(0.3, min(0.8, brake_strength))
-
- if distance < required_distance:
- throttle = 0.0
- brake = brake_strength
- else:
- throttle = 0.1
- brake = 0.0
-
- # 尝试变道
- if abs(angle) < 15:
- location = vehicle.get_location()
- waypoint = self.map.get_waypoint(location)
- left_lane = waypoint.get_left_lane()
- right_lane = waypoint.get_right_lane()
-
- if left_lane and left_lane.lane_type == carla.LaneType.Driving:
- steer = -0.3
- elif right_lane and right_lane.lane_type == carla.LaneType.Driving:
- steer = 0.3
- else:
- steer = 0.2 if angle >= 0 else -0.2
+# ===================== 全局配置 ======================
+# CARLA连接
+CARLA_HOST = "localhost"
+CARLA_PORT = 2000
+CARLA_TIMEOUT = 10.0
+
+# 车辆配置
+VEHICLE_MODEL = "vehicle.tesla.model3"
+VEHICLE_WHEELBASE = 2.9
+VEHICLE_REAR_AXLE_OFFSET = 1.45
+
+# 转向控制
+LOOKAHEAD_DIST_STRAIGHT = 7.0
+LOOKAHEAD_DIST_CURVE = 4.0
+STEER_GAIN_STRAIGHT = 0.7
+STEER_GAIN_CURVE = 1.0
+STEER_DEADZONE = 0.05
+STEER_LOWPASS_ALPHA = 0.6
+MAX_STEER = 1.0
+
+# 弯道等级
+DIR_CHANGE_GENTLE = 0.03
+DIR_CHANGE_SHARP = 0.08
+
+# 速度控制
+BASE_SPEED = 25.0
+PID_KP = 0.2
+PID_KI = 0.01
+PID_KD = 0.02
+
+# 相机
+CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
+CAMERA_WIDTH = 800
+CAMERA_HEIGHT = 600
+CAMERA_FOV = 90
+
+# 交通规则
+TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
+TRAFFIC_LIGHT_DETECTION_RANGE = 50.0 # 扩大检测范围，能检测更多交通灯
+TRAFFIC_LIGHT_ANGLE_THRESHOLD = 60.0 # 扩大检测角度
+STOP_SPEED_THRESHOLD = 0.2
+GREEN_LIGHT_ACCEL_FACTOR = 0.25
+STOP_LINE_SIM_DISTANCE = 5.0
+
+# 交通灯状态循环配置（单位：秒）
+RED_LIGHT_DURATION = 3.0
+GREEN_LIGHT_DURATION = 5.0
+YELLOW_LIGHT_DURATION = 2.0
+
+# 车道行驶配置
+ROAD_DIRECTION_DOT_THRESHOLD = 0.0
+LANE_KEEP_STRICTNESS = 1.2
+
+# ===================== 核心兼容工具函数 ======================
+def is_actor_alive(actor):
+ """兼容不同版本的Actor存活状态判断"""
+ try:
+ return actor.is_alive()
+ except TypeError:
+ return actor.is_alive
- return throttle, brake, steer
+def get_traffic_light_stop_line(traffic_light):
+ """兼容不同版本的交通灯停止线获取"""
+ try:
+ return traffic_light.get_stop_line_location()
+ except AttributeError:
+ tl_transform = traffic_light.get_transform()
+ forward_vec = tl_transform.get_forward_vector()
+ stop_line_loc = tl_transform.location - forward_vec * STOP_LINE_SIM_DISTANCE
+ stop_line_loc.z = tl_transform.location.z
+ return stop_line_loc
+
+def get_spawn_point_near_traffic_light(world, map):
+ """自动查找距离交通灯最近的出生点"""
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ if not traffic_lights:
+ print("警告：当前地图中未找到交通灯，使用默认出生点")
+ spawn_points = map.get_spawn_points()
+ return spawn_points[0] if spawn_points else carla.Transform()
+
+ spawn_points = map.get_spawn_points()
+ if not spawn_points:
+ print("警告：未找到默认出生点，使用交通灯旁位置")
+ tl_transform = traffic_lights[0].get_transform()
+ return carla.Transform(tl_transform.location + carla.Location(x=-5.0, z=0.5), tl_transform.rotation)
+
+ min_distance = float('inf')
+ best_spawn_point = spawn_points[0]
+
+ for spawn_point in spawn_points:
+ for tl in traffic_lights:
+ if not is_actor_alive(tl):
+ continue
+ tl_loc = tl.get_transform().location
+ spawn_loc = spawn_point.location
+ distance = math.sqrt((tl_loc.x - spawn_loc.x)**2 + (tl_loc.y - spawn_loc.y)**2)
+ if distance < min_distance:
+ min_distance = distance
+ best_spawn_point = spawn_point
+
+ print(f"找到距离交通灯最近的出生点，距离：{min_distance:.2f}米")
+ return best_spawn_point
+
+def cycle_traffic_light_states(world):
+ """
+ 循环控制所有交通灯的状态：红→绿→黄→红
+ 作为后台线程运行，确保交通灯持续变化
+ """
+ while True:
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ # 设置所有交通灯为红灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Red)
+ except:
+ pass
+ time.sleep(RED_LIGHT_DURATION)
+
+ # 设置所有交通灯为绿灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Green)
+ except:
+ pass
+ time.sleep(GREEN_LIGHT_DURATION)
+
+ # 设置所有交通灯为黄灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Yellow)
+ except:
+ pass
+ time.sleep(YELLOW_LIGHT_DURATION)
+
+# ===================== 纯追踪控制器 =====================
+class AdaptivePurePursuit:
+ def __init__(self, wheelbase):
+ self.wheelbase = wheelbase
+ self.last_steer = 0.0
+ self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT
+
+ def calculate_steer(self, vehicle_transform, target_point, dir_change):
+ # 1. 后轴位置
+ forward_vec = vehicle_transform.get_forward_vector()
+ rear_axle_loc = carla.Location(
+ x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
+ y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
+ z=vehicle_transform.location.z
+ )
- def get_control(self, vehicle):
- """生成传统控制指令（优先避障，弱化基础速度控制）"""
- # 获取车辆状态
- transform = vehicle.get_transform()
- location = vehicle.get_location()
- velocity = vehicle.get_velocity()
- speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6
+ # 2. 车辆坐标系转换
+ dx = target_point.x - rear_axle_loc.x
+ dy = target_point.y - rear_axle_loc.y
+ yaw = math.radians(vehicle_transform.rotation.yaw)
- # 获取障碍物信息
- obstacle_info = self.obstacle_detector.get_obstacle_info()
+ dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
+ dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
- # 获取路点
- waypoint = self.map.get_waypoint(location, project_to_road=True)
- next_waypoints = waypoint.next(self.waypoint_distance)
- target_waypoint = next_waypoints[0] if next_waypoints else waypoint
+ # 3. 转向增益
+ steer_gain = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
+ )
+ steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
- # 计算转向
- vehicle_yaw = math.radians(transform.rotation.yaw)
- target_loc = target_waypoint.transform.location
+ # 4. 纯追踪计算
+ if dx_vehicle < 0.1:
+ steer = self.last_steer
+ else:
+ steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * LANE_KEEP_STRICTNESS, dx_vehicle ** 2 + dy_vehicle ** 2)
+ steer = steer_rad / math.pi
+ steer *= steer_gain
- dx = target_loc.x - location.x
- dy = target_loc.y - location.y
+ # 5. 死区+滤波
+ if abs(steer) < STEER_DEADZONE:
+ steer = 0.0
+ steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
+ steer = np.clip(steer, -MAX_STEER, MAX_STEER)
- local_x = dx * math.cos(vehicle_yaw) + dy * math.sin(vehicle_yaw)
- local_y = -dx * math.sin(vehicle_yaw) + dy * math.cos(vehicle_yaw)
+ self.last_steer = steer
+ return steer
- if abs(local_x) < 0.1:
- steer = 0.0
+ def get_adaptive_lookahead(self, dir_change):
+ lookahead_dist = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
+ )
+ lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
+ self.last_lookahead = lookahead_dist
+ return lookahead_dist
+
+# ===================== 速度控制器 =====================
+class SpeedController:
+ def __init__(self, kp, ki, kd):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.last_error = 0.0
+ self.integral = 0.0
+
+ def calculate(self, target_speed, current_speed):
+ error = target_speed - current_speed
+ p = self.kp * error
+ self.integral += self.ki * error
+ self.integral = np.clip(self.integral, -1.0, 1.0)
+ i = self.integral
+ d = self.kd * (error - self.last_error)
+ self.last_error = error
+ return np.clip(p + i + d, 0.0, 1.0)
+
+# ===================== 交通灯管理类 ======================
+class TrafficLightManager:
+ def __init__(self):
+ self.tracked_light = None
+ self.is_stopped_at_red = False
+ self.red_light_stop_time = 0
+
+ def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_transform):
+ """计算车辆前进方向与交通灯的夹角"""
+ vehicle_forward = vehicle_transform.get_forward_vector()
+ vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y])
+ vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward)
+
+ light_dir = light_transform.location - vehicle_transform.location
+ light_dir = np.array([light_dir.x, light_dir.y])
+ if np.linalg.norm(light_dir) < 0.1:
+ return 0.0
+ light_dir = light_dir / np.linalg.norm(light_dir)
+
+ angle = math.acos(np.clip(np.dot(vehicle_forward, light_dir), -1.0, 1.0))
+ angle = math.degrees(angle)
+ return angle
+
+ def get_lane_traffic_light(self, vehicle, world):
+ """扩大检测范围，检测更多交通灯"""
+ vehicle_transform = vehicle.get_transform()
+ vehicle_loc = vehicle_transform.location
+
+ # 检查跟踪的交通灯是否存活
+ if self.tracked_light and is_actor_alive(self.tracked_light):
+ dist = self.tracked_light.get_transform().location.distance(vehicle_loc)
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, self.tracked_light.get_transform())
+ if dist < TRAFFIC_LIGHT_DETECTION_RANGE and angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ return self.tracked_light
+
+ # 获取所有交通灯并筛选有效灯
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ valid_lights = []
+
+ for light in traffic_lights:
+ if not is_actor_alive(light):
+ continue
+ dist = light.get_transform().location.distance(vehicle_loc)
+ if dist > TRAFFIC_LIGHT_DETECTION_RANGE:
+ continue
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, light.get_transform())
+ if angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ valid_lights.append((dist, light))
+
+ if valid_lights:
+ valid_lights.sort(key=lambda x: x[0])
+ self.tracked_light = valid_lights[0][1]
+ return self.tracked_light
+
+ self.tracked_light = None
+ return None
+
+ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
+ """红灯强制停车，绿灯恢复行驶"""
+ world = vehicle.get_world()
+ traffic_light = self.get_lane_traffic_light(vehicle, world)
+
+ if not traffic_light:
+ self.is_stopped_at_red = False
+ self.red_light_stop_time = 0
+ return base_target_speed, "No Light (Lane)"
+
+ # 核心：使用兼容函数获取停止线位置
+ stop_line_loc = get_traffic_light_stop_line(traffic_light)
+ dist_to_stop_line = vehicle.get_transform().location.distance(stop_line_loc)
+
+ if traffic_light.get_state() == carla.TrafficLightState.Green:
+ if self.is_stopped_at_red:
+ recovery_speed = current_speed + (base_target_speed - current_speed) * GREEN_LIGHT_ACCEL_FACTOR
+ target_speed = max(STOP_SPEED_THRESHOLD, recovery_speed)
+ if abs(target_speed - base_target_speed) < 0.5:
+ self.is_stopped_at_red = False
+ return target_speed, "Green (Recovering)"
+ return base_target_speed, "Green (Lane)"
+
+ elif traffic_light.get_state() == carla.TrafficLightState.Yellow:
+ self.is_stopped_at_red = False
+ yellow_speed = max(5.0, base_target_speed * 0.3)
+ return yellow_speed, "Yellow (Stop Soon)"
+
+ elif traffic_light.get_state() == carla.TrafficLightState.Red:
+ if dist_to_stop_line > TRAFFIC_LIGHT_STOP_DISTANCE:
+ self.is_stopped_at_red = False
+ red_speed = max(2.0, current_speed * 0.1)
+ return red_speed, f"Red (Decelerating: {dist_to_stop_line:.1f}m)"
+ else:
+ if current_speed <= STOP_SPEED_THRESHOLD:
+ self.is_stopped_at_red = True
+ self.red_light_stop_time += 1
+ wait_seconds = self.red_light_stop_time // 30
+ return 0.0, f"Red (Stopped: {wait_seconds}s)"
+ else:
+ return 0.0, "Red (Emergency Stop)"
+
+ return base_target_speed, "Unknown Light"
+
+# ===================== 车道行驶辅助函数 ======================
+def calculate_dir_change(current_wp):
+ """计算方向变化量，判断弯道等级"""
+ waypoints = [current_wp]
+ for i in range(5):
+ next_wps = waypoints[-1].next(1.0)
+ if next_wps:
+ waypoints.append(next_wps[0])
+ else:
+ break
+
+ if len(waypoints) < 4:
+ return 0.0, 0
+
+ dirs = []
+ for i in range(1, len(waypoints)):
+ wp_prev = waypoints[i-1]
+ wp_curr = waypoints[i]
+ dir_rad = math.atan2(
+ wp_curr.transform.location.y - wp_prev.transform.location.y,
+ wp_curr.transform.location.x - wp_prev.transform.location.x
+ )
+ dirs.append(dir_rad)
+
+ dir_change = 0.0
+ for i in range(1, len(dirs)):
+ dir_change += abs(dirs[i] - dirs[i-1]) * 2
+
+ if dir_change < DIR_CHANGE_GENTLE:
+ curve_level = 0
+ elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 1
+ else:
+ curve_level = 2
+
+ return dir_change, curve_level
+
+def get_forward_waypoint(vehicle, map):
+ """获取车辆当前车道的前进方向路点（极低版本兼容）"""
+ vehicle_transform = vehicle.get_transform()
+ # 1. 投影到道路
+ current_wp = map.get_waypoint(
+ vehicle_transform.location,
+ project_to_road=True
+ )
+
+ # 2. 纯数学判断方向是否相反
+ road_direction = current_wp.transform.get_forward_vector()
+ vehicle_direction = vehicle_transform.get_forward_vector()
+ dot_product = road_direction.x * vehicle_direction.x + road_direction.y * vehicle_direction.y
+
+ # 3. 方向相反则取前方点
+ if dot_product < ROAD_DIRECTION_DOT_THRESHOLD:
+ forward_wps = current_wp.next(10.0)
+ if forward_wps:
+ current_wp = forward_wps[0]
else:
- angle = math.atan2(local_y, local_x)
- steer = np.clip(angle / math.radians(45), -1.0, 1.0)
-
- # 基础速度控制（降低优先级）
- if speed < 20:
- throttle = 0.4 # 从0.6降低到0.4，减少油门
- brake = 0.0
- elif speed < 40:
- throttle = 0.2 # 从0.4降低到0.2
- brake = 0.0
+ current_wp = map.get_waypoint(
+ vehicle_transform.location + vehicle_direction * 5.0,
+ project_to_road=True
+ )
+
+ return current_wp
+
+# ===================== 相机管理器 =====================
+class CameraManager:
+ def __init__(self, world, vehicle, display):
+ self.world = world
+ self.vehicle = vehicle
+ self.display = display
+ self.camera = None
+ self.traffic_light_status = "No Light"
+ self._create_camera()
+
+ def _create_camera(self):
+ bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
+ bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
+ bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
+ bp.set_attribute("fov", str(CAMERA_FOV))
+ self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
+ self.camera.listen(self._on_image)
+
+ def _on_image(self, image):
+ """修正数组处理逻辑"""
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))
+ array = array[:, :, :3]
+ array = array[:, :, ::-1].swapaxes(0, 1)
+ self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
+ self._draw_info()
+ pygame.display.flip()
+
+ def _draw_info(self):
+ font = pygame.font.SysFont("Arial", 24, bold=True)
+ if "Red" in self.traffic_light_status:
+ color = (255, 0, 0)
+ elif "Green" in self.traffic_light_status:
+ color = (0, 255, 0)
+ elif "Yellow" in self.traffic_light_status:
+ color = (255, 255, 0)
else:
- throttle = 0.1
- brake = 0.2
+ color = (255, 255, 255)
- # 障碍物调整（优先执行）
- throttle, brake, steer = self.apply_obstacle_avoidance(throttle, brake, steer, vehicle, obstacle_info)
+ text = font.render(f"Traffic Light: {self.traffic_light_status}", True, color)
+ bg = pygame.Surface((text.get_width() + 10, text.get_height() + 5))
+ bg.fill((0, 0, 0))
+ self.display.blit(bg, (5, 5))
+ self.display.blit(text, (10, 7))
- # 低速强油门（仅当无障碍物时生效）
- if speed < 5.0 and not obstacle_info['has_obstacle']:
- throttle = 0.4
- brake = 0.0
+ def update_traffic_light_status(self, status):
+ self.traffic_light_status = status
- return throttle, brake, steer
+ def destroy(self):
+ if self.camera:
+ if is_actor_alive(self.camera):
+ self.camera.stop()
+ self.camera.destroy()
-# --------------------------
-# 3. CARLA环境初始化（适配0.9.10）
-# --------------------------
+# ===================== 主函数（核心：多交通灯配置）=====================
def main():
+ pygame.init()
+ display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
+ pygame.display.set_caption("CARLA 多交通灯版（Town04密集交通灯+状态循环）")
+
+ client = None
+ world = None
+ vehicle = None
+ camera_manager = None
+ pp_controller = None
+ speed_controller = None
+ traffic_light_manager = None
+
try:
- # 连接CARLA服务器（0.9.10兼容）
- client = carla.Client('localhost', 2000)
- client.set_timeout(15.0)
- world = client.load_world('Town01') # 0.9.10支持Town01
-
- # 设置同步模式（0.9.10关键配置）
- settings = world.get_settings()
- settings.synchronous_mode = True
- settings.fixed_delta_seconds = 0.1
- world.apply_settings(settings)
-
- # 设置天气
- weather = carla.WeatherParameters(
- cloudiness=30.0,
- precipitation=0.0,
- sun_altitude_angle=70.0
+ # 1. 连接CARLA，加载**Town04**（交通灯最密集的地图）
+ client = carla.Client(CARLA_HOST, CARLA_PORT)
+ client.set_timeout(CARLA_TIMEOUT)
+ try:
+ client.load_world("Town04") # 替换为Town04，交通灯数量远多于Town03
+ except:
+ world = client.get_world()
+ print("警告：Town04地图不存在，使用当前地图")
+ else:
+ world = client.get_world()
+ print("成功加载Town04地图（交通灯密集）")
+ map = world.get_map()
+
+ # 2. 清理残留演员
+ for actor in world.get_actors():
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.", "controller.")):
+ if is_actor_alive(actor):
+ actor.destroy()
+ print("残留演员清理完成")
+
+ # 3. 启动交通灯状态循环线程（后台持续切换交通灯状态）
+ import threading
+ tl_cycle_thread = threading.Thread(target=cycle_traffic_light_states, args=(world,), daemon=True)
+ tl_cycle_thread.start()
+ print("交通灯状态循环线程已启动（红3秒→绿5秒→黄2秒）")
+
+ # 4. 设置车辆起始位置（Town04交通灯密集区）
+ vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
+
+ # 手动指定Town04的核心交通灯路口坐标（经测试：多个交通灯环绕）
+ spawn_point = carla.Transform(
+ carla.Location(x=220.0, y=150.0, z=0.5), # Town04核心交通灯密集区
+ carla.Rotation(yaw=90.0)
)
- world.set_weather(weather)
+ spawn_point.location.z += 0.2
+ vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
- # 获取出生点
- map = world.get_map()
- spawn_points = map.get_spawn_points()
- if not spawn_points:
- raise Exception("无可用出生点")
- spawn_point = spawn_points[10]
-
- # 生成主车辆
- blueprint_library = world.get_blueprint_library()
- vehicle_bp = blueprint_library.find('vehicle.tesla.model3')
- vehicle_bp.set_attribute('color', '255,0,0')
- vehicle = world.spawn_actor(vehicle_bp, spawn_point)
+ # 备用方案：自动查找交通灯附近的出生点
if not vehicle:
- raise Exception("无法生成主车辆")
- vehicle.set_autopilot(False)
- print(f"车辆生成在位置: {spawn_point.location}")
-
- # 生成障碍物车辆（调整生成位置，确保在前车前方）
- obstacle_count = 3
- for i in range(obstacle_count):
- spawn_idx = (i + 12) % len(spawn_points) # 从15改为12，更靠近主车辆
- other_vehicle_bp = random.choice(blueprint_library.filter('vehicle.*'))
- other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx])
- if other_vehicle:
- other_vehicle.set_autopilot(True)
-
- # 配置传感器（0.9.10兼容）
- # 后视角相机
- third_camera_bp = blueprint_library.find('sensor.camera.rgb')
- third_camera_bp.set_attribute('image_size_x', '640')
- third_camera_bp.set_attribute('image_size_y', '480')
- third_camera_bp.set_attribute('fov', '110')
- third_camera_transform = carla.Transform(
- carla.Location(x=-5.0, y=0.0, z=3.0),
- carla.Rotation(pitch=-15.0)
- )
- third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle)
-
- # 前视角相机
- front_camera_bp = blueprint_library.find('sensor.camera.rgb')
- front_camera_bp.set_attribute('image_size_x', '640')
- front_camera_bp.set_attribute('image_size_y', '480')
- front_camera_bp.set_attribute('fov', '90')
- front_camera_transform = carla.Transform(
- carla.Location(x=2.0, y=0.0, z=1.5),
- carla.Rotation(pitch=0.0)
- )
- front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle)
-
- # 传感器数据存储
- third_image = None
- front_image = None
-
- # 传感器回调函数
- def third_camera_callback(image):
- nonlocal third_image
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = np.reshape(array, (image.height, image.width, 4))
- third_image = array[:, :, :3]
-
- def front_camera_callback(image):
- nonlocal front_image
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = np.reshape(array, (image.height, image.width, 4))
- front_image = array[:, :, :3]
-
- # 注册回调
- third_camera.listen(third_camera_callback)
- front_camera.listen(front_camera_callback)
- time.sleep(2.0) # 等待传感器初始化
-
- # 初始化核心组件
- obstacle_detector = ObstacleDetector(world, vehicle)
- traditional_controller = TraditionalController(world, obstacle_detector)
-
- # 控制变量
- throttle = 0.3
- steer = 0.0
- brake = 0.0
- frame_count = 0
- stuck_count = 0
- last_position = vehicle.get_location()
-
- # 主循环
- print("自动驾驶系统启动 - 仅使用传统控制器")
- print("控制键: q-退出, r-重置车辆")
-
- while True:
- world.tick()
- frame_count += 1
+ print("手动坐标生成失败，自动查找交通灯附近的出生点...")
+ spawn_point = get_spawn_point_near_traffic_light(world, map)
+ vehicle = world.spawn_actor(vehicle_bp, spawn_point)
+
+ # 最终备用方案
+ if not vehicle:
+ spawn_points = map.get_spawn_points()
+ spawn_point = spawn_points[0] if spawn_points else carla.Transform()
+ vehicle = world.spawn_actor(vehicle_bp, spawn_point)
+
+ print(f"车辆生成成功：{vehicle.type_id}（起始于交通灯密集区）")
+
+ # 5. 初始化组件
+ pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
+ traffic_light_manager = TrafficLightManager()
+ camera_manager = CameraManager(world, vehicle, display)
+
+ # 6. 主循环
+ print("仿真启动，按ESC退出...（车辆将经过多个交通灯）")
+ clock = pygame.time.Clock()
+ running = True
+
+ while running:
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
+ running = False
# 获取车辆状态
vehicle_transform = vehicle.get_transform()
- vehicle_location = vehicle.get_transform().location
- vehicle_velocity = vehicle.get_velocity()
- vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2)
-
- # 检测障碍物
- obstacle_info = obstacle_detector.get_obstacle_info()
-
- # 卡住检测（优化：有障碍物时不触发）
- distance_moved = vehicle_location.distance(last_position)
- is_moving = distance_moved > 0.2 or vehicle_speed > 1.0
- # 关键修正：通过traditional_controller实例访问属性，而非self
- if obstacle_info['has_obstacle'] and obstacle_info['distance'] < traditional_controller.safe_following_distance:
- stuck_count = 0
- else:
- if not is_moving:
- stuck_count += 1
- else:
- stuck_count = 0
- last_position = vehicle_location
-
- # 卡住恢复（仅当无障碍物时执行）
- if stuck_count > 20: # 从15帧改为20帧，降低误触发
- print("检测到车辆卡住，执行恢复程序...")
- # 紧急刹车
- vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0, brake=1.0, hand_brake=True))
- time.sleep(0.5)
- # 倒车或转向
- if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 15:
- vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0, reverse=True))
- time.sleep(1.0)
- else:
- recovery_steer = random.choice([-0.5, 0.5])
- vehicle.apply_control(carla.VehicleControl(throttle=0.6, steer=recovery_steer, brake=0.0)) # 降低油门到0.6
- time.sleep(1.0)
- stuck_count = 0
-
- # 生成控制指令（仅使用传统控制器）
- throttle, brake, steer = traditional_controller.get_control(vehicle)
-
- # 紧急刹车时拉手刹
- if brake >= 1.0:
- vehicle.apply_control(carla.VehicleControl(
- throttle=0.0, steer=steer, brake=1.0, hand_brake=True
- ))
- else:
- # 应用控制
- control = carla.VehicleControl(
- throttle=throttle,
- steer=steer,
- brake=brake,
- hand_brake=False,
- reverse=False
- )
- vehicle.apply_control(control)
-
- # 图像显示
- if third_image is not None:
- display_image = third_image.copy()
- # 可视化障碍物
- display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform)
- # 绘制信息
- cv2.putText(display_image, f"Speed: {vehicle_speed * 3.6:.1f} km/h", (10, 30),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- cv2.putText(display_image, f"Mode: Traditional", (10, 60),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- # 障碍物信息
- if obstacle_info['has_obstacle']:
- cv2.putText(display_image, f"Obstacle: {obstacle_info['distance']:.1f}m", (10, 180),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
- cv2.putText(display_image, f"RelSpeed: {obstacle_info['relative_speed']:.1f}km/h", (10, 210),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
- else:
- cv2.putText(display_image, "Obstacle: None", (10, 180),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- # 卡住警告
- if stuck_count > 5:
- cv2.putText(display_image, "STUCK DETECTED!", (10, 240),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
-
- cv2.imshow('CARLA Autopilot (0.9.10)', display_image)
-
- # 键盘控制
- key = cv2.waitKey(1) & 0xFF
- if key == ord('q'):
- break
- elif key == ord('r'):
- vehicle.set_transform(spawn_point)
- throttle = 0.3
- steer = 0.0
- brake = 0.0
- stuck_count = 0
- print("车辆已重置")
-
- time.sleep(0.01)
+ vehicle_vel = vehicle.get_velocity()
+ current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6
+
+ # 核心：获取前进方向路点
+ current_wp = get_forward_waypoint(vehicle, map)
+
+ # 计算方向变化
+ dir_change, curve_level = calculate_dir_change(current_wp)
+
+ # 预瞄距离
+ lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)
+
+ # 目标路点
+ target_wps = current_wp.next(lookahead_dist)
+ target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+
+ # 基础速度
+ curve_speed_factors = [1.0, 0.7, 0.4]
+ speed_factor = curve_speed_factors[min(curve_level, 2)]
+ base_target_speed = BASE_SPEED * speed_factor
+ base_target_speed = max(8.0, base_target_speed)
+
+ # 交通灯处理（检测多个交通灯）
+ target_speed, traffic_light_status = traffic_light_manager.handle_traffic_light_logic(
+ vehicle, current_speed, base_target_speed
+ )
+ camera_manager.update_traffic_light_status(traffic_light_status)
+
+ # 控制计算
+ steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
+ throttle = speed_controller.calculate(target_speed, current_speed)
+ brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
+
+ # 红灯刹车
+ if "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
+ throttle = 0.0
+ brake = 1.0
+
+ # 应用控制
+ control = carla.VehicleControl()
+ control.steer = steer
+ control.throttle = throttle
+ control.brake = brake
+ vehicle.apply_control(control)
+
+ # 打印状态（显示当前交通灯状态）
+ curve_names = ["直道", "缓弯", "急弯"]
+ lane_id = current_wp.lane_id
+ print(f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | 弯道：{curve_names[curve_level]:<3} | 车道ID：{lane_id} | 灯状态：{traffic_light_status}")
+
+ clock.tick(30)
except Exception as e:
- print(f"系统错误: {e}")
- import traceback
+ print(f"错误：{e}")
traceback.print_exc()
finally:
- # 清理资源（0.9.10兼容）
- print("正在清理资源...")
- cv2.destroyAllWindows()
- # 停止传感器
- if 'third_camera' in locals():
- third_camera.stop()
- if 'front_camera' in locals():
- front_camera.stop()
- # 销毁所有Actor
- for actor in world.get_actors():
- if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'):
- actor.destroy()
- # 关闭同步模式
- settings = world.get_settings()
- settings.synchronous_mode = False
- world.apply_settings(settings)
- print("资源清理完成")
+ print("清理资源...")
+ if camera_manager:
+ camera_manager.destroy()
+ if vehicle and is_actor_alive(vehicle):
+ vehicle.destroy()
+ pygame.quit()
+ print("仿真结束")
if __name__ == "__main__":
main()
\ No newline at end of file
From 914f120ff3bb1ba7f6e8ee8addd9f63bd8be7096 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sun, 21 Dec 2025 15:18:24 +0800
Subject: [PATCH 14/26] =?UTF-8?q?=E5=A4=9A=E8=BD=A6=E8=BE=86=E5=8D=8F?=
=?UTF-8?q?=E5=90=8C=E6=8E=A7=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 976 +++++++++++++++++++----------
1 file changed, 642 insertions(+), 334 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index e86acaf6c1..533eccd517 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
-CARLA 多交通灯版：Town04密集交通灯+状态循环+车辆响应
+CARLA 多车辆协同控制版：修复出生点索引越界问题
"""
import sys
@@ -12,19 +12,32 @@
import pygame
import traceback
import time
+import threading
+from concurrent.futures import ThreadPoolExecutor, as_completed
+import logging
+import random
-# ===================== 全局配置 ======================
+# ===================== 全局配置 =======================
# CARLA连接
CARLA_HOST = "localhost"
CARLA_PORT = 2000
-CARLA_TIMEOUT = 10.0
-
-# 车辆配置
-VEHICLE_MODEL = "vehicle.tesla.model3"
+CARLA_TIMEOUT = 20.0
+
+# 多车辆配置
+VEHICLE_COUNT = 3
+VEHICLE_MODELS = [
+ "vehicle.tesla.model3",
+ "vehicle.bmw.grandtourer",
+ "vehicle.audi.a2"
+]
+SPAWN_INTERVAL = 1.0
+SPAWN_RETRY_MAX = 8
+SPAWN_RETRY_DELAY = 0.5
+SPAWN_DISTANCE_LIMIT = 15.0 # 放宽距离限制到15米
+
+# 车辆控制参数
VEHICLE_WHEELBASE = 2.9
VEHICLE_REAR_AXLE_OFFSET = 1.45
-
-# 转向控制
LOOKAHEAD_DIST_STRAIGHT = 7.0
LOOKAHEAD_DIST_CURVE = 4.0
STEER_GAIN_STRAIGHT = 0.7
@@ -32,50 +45,49 @@
STEER_DEADZONE = 0.05
STEER_LOWPASS_ALPHA = 0.6
MAX_STEER = 1.0
-
-# 弯道等级
DIR_CHANGE_GENTLE = 0.03
DIR_CHANGE_SHARP = 0.08
-
-# 速度控制
-BASE_SPEED = 25.0
+BASE_SPEEDS = [25.0, 22.0, 20.0]
PID_KP = 0.2
PID_KI = 0.01
PID_KD = 0.02
-# 相机
-CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
-CAMERA_WIDTH = 800
-CAMERA_HEIGHT = 600
-CAMERA_FOV = 90
-
-# 交通规则
+# 交通规则配置
TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
-TRAFFIC_LIGHT_DETECTION_RANGE = 50.0 # 扩大检测范围，能检测更多交通灯
-TRAFFIC_LIGHT_ANGLE_THRESHOLD = 60.0 # 扩大检测角度
+TRAFFIC_LIGHT_DETECTION_RANGE = 50.0
+TRAFFIC_LIGHT_ANGLE_THRESHOLD = 60.0
STOP_SPEED_THRESHOLD = 0.2
GREEN_LIGHT_ACCEL_FACTOR = 0.25
STOP_LINE_SIM_DISTANCE = 5.0
-
-# 交通灯状态循环配置（单位：秒）
RED_LIGHT_DURATION = 3.0
GREEN_LIGHT_DURATION = 5.0
YELLOW_LIGHT_DURATION = 2.0
-# 车道行驶配置
-ROAD_DIRECTION_DOT_THRESHOLD = 0.0
-LANE_KEEP_STRICTNESS = 1.2
+# 相机配置
+WINDOW_WIDTH = 1280
+WINDOW_HEIGHT = 720
+CAMERA_FOV = 120
+CAMERA_POS = carla.Transform(carla.Location(x=-6.0, z=2.5), carla.Rotation(pitch=-5))
+
+# 全局变量
+current_view_vehicle_id = 1
+vehicle_agents = []
-# ===================== 核心兼容工具函数 ======================
+# 日志配置
+logging.basicConfig(
+ level=logging.INFO,
+ format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
+ handlers=[logging.FileHandler("multi_vehicle_simulation.log"), logging.StreamHandler()]
+)
+
+# ===================== 核心工具函数 ======================
def is_actor_alive(actor):
- """兼容不同版本的Actor存活状态判断"""
try:
return actor.is_alive()
except TypeError:
return actor.is_alive
def get_traffic_light_stop_line(traffic_light):
- """兼容不同版本的交通灯停止线获取"""
try:
return traffic_light.get_stop_line_location()
except AttributeError:
@@ -85,72 +97,363 @@ def get_traffic_light_stop_line(traffic_light):
stop_line_loc.z = tl_transform.location.z
return stop_line_loc
-def get_spawn_point_near_traffic_light(world, map):
- """自动查找距离交通灯最近的出生点"""
- traffic_lights = world.get_actors().filter("traffic.traffic_light")
- if not traffic_lights:
- print("警告：当前地图中未找到交通灯，使用默认出生点")
- spawn_points = map.get_spawn_points()
- return spawn_points[0] if spawn_points else carla.Transform()
+def calculate_dir_change(current_wp):
+ waypoints = [current_wp]
+ for i in range(5):
+ next_wps = waypoints[-1].next(1.0)
+ if next_wps:
+ waypoints.append(next_wps[0])
+ else:
+ break
- spawn_points = map.get_spawn_points()
- if not spawn_points:
- print("警告：未找到默认出生点，使用交通灯旁位置")
- tl_transform = traffic_lights[0].get_transform()
- return carla.Transform(tl_transform.location + carla.Location(x=-5.0, z=0.5), tl_transform.rotation)
+ if len(waypoints) < 4:
+ return 0.0, 0
- min_distance = float('inf')
- best_spawn_point = spawn_points[0]
+ dirs = []
+ for i in range(1, len(waypoints)):
+ wp_prev = waypoints[i-1]
+ wp_curr = waypoints[i]
+ dir_rad = math.atan2(
+ wp_curr.transform.location.y - wp_prev.transform.location.y,
+ wp_curr.transform.location.x - wp_prev.transform.location.x
+ )
+ dirs.append(dir_rad)
- for spawn_point in spawn_points:
- for tl in traffic_lights:
- if not is_actor_alive(tl):
- continue
- tl_loc = tl.get_transform().location
- spawn_loc = spawn_point.location
- distance = math.sqrt((tl_loc.x - spawn_loc.x)**2 + (tl_loc.y - spawn_loc.y)**2)
- if distance < min_distance:
- min_distance = distance
- best_spawn_point = spawn_point
+ dir_change = 0.0
+ for i in range(1, len(dirs)):
+ dir_change += abs(dirs[i] - dirs[i-1]) * 2
- print(f"找到距离交通灯最近的出生点，距离：{min_distance:.2f}米")
- return best_spawn_point
+ if dir_change < DIR_CHANGE_GENTLE:
+ curve_level = 0
+ elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 1
+ else:
+ curve_level = 2
+
+ return dir_change, curve_level
+
+def get_forward_waypoint(vehicle, map):
+ vehicle_transform = vehicle.get_transform()
+ current_wp = map.get_waypoint(
+ vehicle_transform.location,
+ project_to_road=True,
+ lane_type=carla.LaneType.Driving
+ )
+
+ # 所有车辆使用同一车道的路点
+ global vehicle_agents
+ if len(vehicle_agents) > 0:
+ try:
+ lead_vehicle_wp = map.get_waypoint(vehicle_agents[0].vehicle.get_transform().location, project_to_road=True)
+ current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True, lane_id=lead_vehicle_wp.lane_id)
+ except:
+ pass
+
+ road_direction = current_wp.transform.get_forward_vector()
+ vehicle_direction = vehicle_transform.get_forward_vector()
+ dot_product = road_direction.x * vehicle_direction.x + road_direction.y * vehicle_direction.y
-def cycle_traffic_light_states(world):
+ if dot_product < 0.0:
+ forward_wps = current_wp.next(10.0)
+ if forward_wps:
+ current_wp = forward_wps[0]
+ else:
+ current_wp = map.get_waypoint(
+ vehicle_transform.location + vehicle_direction * 5.0,
+ project_to_road=True
+ )
+
+ return current_wp
+
+def get_valid_spawn_points(map, count, base_location=None, radius=100.0):
"""
- 循环控制所有交通灯的状态：红→绿→黄→红
- 作为后台线程运行，确保交通灯持续变化
+ 获取有效的出生点（增加容错性，避免索引越界）
"""
- while True:
- traffic_lights = world.get_actors().filter("traffic.traffic_light")
- # 设置所有交通灯为红灯
- for tl in traffic_lights:
- if is_actor_alive(tl):
- try:
- tl.set_state(carla.TrafficLightState.Red)
- except:
- pass
- time.sleep(RED_LIGHT_DURATION)
+ # 1. 获取地图所有出生点
+ all_spawn_points = map.get_spawn_points()
+ if not all_spawn_points:
+ raise RuntimeError("地图中无任何出生点")
+
+ # 2. 初始化候选点列表
+ candidate_points = []
+
+ # 3. 如果有基准位置，先筛选附近的点；否则直接使用所有点
+ if base_location:
+ filtered_points = []
+ for sp in all_spawn_points:
+ dist = sp.location.distance(base_location)
+ if dist <= radius:
+ filtered_points.append((dist, sp))
+ # 按距离排序
+ filtered_points.sort(key=lambda x: x[0])
+ candidate_points = [sp for _, sp in filtered_points]
+
+ # 4. 如果候选点为空，直接使用所有出生点（容错）
+ if not candidate_points:
+ candidate_points = all_spawn_points
+ print(f"警告：基准位置{base_location}附近无出生点，使用全局出生点")
+
+ # 5. 筛选集中的出生点（放宽条件）
+ valid_points = []
+ # 确保基准点存在（核心修复：避免candidate_points[0]索引越界）
+ if not candidate_points:
+ candidate_points = all_spawn_points
+
+ base_sp = candidate_points[0]
+ valid_points.append(base_sp)
+
+ # 6. 筛选其他点，放宽距离限制
+ for sp in candidate_points[1:]:
+ try:
+ # 检查与已选点的距离（放宽到15米）
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+ except:
+ continue
- # 设置所有交通灯为绿灯
- for tl in traffic_lights:
- if is_actor_alive(tl):
+ # 7. 如果数量不够，进一步放宽条件（距离限制到20米）
+ if len(valid_points) < count:
+ for sp in candidate_points:
+ if sp not in valid_points:
try:
- tl.set_state(carla.TrafficLightState.Green)
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT * 1.5 for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
except:
- pass
- time.sleep(GREEN_LIGHT_DURATION)
+ continue
+
+ # 8. 如果还是不够，直接取前N个点（最终容错）
+ if len(valid_points) < count:
+ print(f"警告：无法找到{count}个集中的出生点，直接取前{count}个可用点")
+ for sp in candidate_points:
+ if sp not in valid_points:
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+
+ # 9. 最终检查：确保数量足够
+ if len(valid_points) < count:
+ # 直接取所有可用点，不足的话重复使用（极端情况）
+ while len(valid_points) < count:
+ valid_points.append(valid_points[0])
+ print(f"警告：出生点数量不足，重复使用已有点")
+
+ # 10. 统一出生点朝向
+ try:
+ forward_vec = valid_points[0].transform.get_forward_vector()
+ for sp in valid_points:
+ sp.rotation.yaw = math.degrees(math.atan2(forward_vec.y, forward_vec.x))
+ except:
+ pass
+
+ return valid_points[:count]
+
+def check_spawn_collision(world, spawn_point, radius=3.0):
+ # 检查周围车辆和行人
+ vehicles = world.get_actors().filter("vehicle.*")
+ for vehicle in vehicles:
+ if is_actor_alive(vehicle):
+ dist = vehicle.get_transform().location.distance(spawn_point.location)
+ if dist < radius:
+ return False
+
+ walkers = world.get_actors().filter("walker.*")
+ for walker in walkers:
+ if is_actor_alive(walker):
+ dist = walker.get_transform().location.distance(spawn_point.location)
+ if dist < radius:
+ return False
+
+ return True
+
+# ===================== 相机管理类（多车辆）=====================
+class VehicleCamera:
+ def __init__(self, world, vehicle, vehicle_id):
+ self.world = world
+ self.vehicle = vehicle
+ self.vehicle_id = vehicle_id
+ self.camera = None
+ self.image_surface = None
- # 设置所有交通灯为黄灯
- for tl in traffic_lights:
- if is_actor_alive(tl):
+ # 创建相机传感器
+ self._create_camera()
+
+ def _create_camera(self):
+ # 加载相机蓝图
+ camera_bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
+ camera_bp.set_attribute("image_size_x", str(640))
+ camera_bp.set_attribute("image_size_y", str(360))
+ camera_bp.set_attribute("fov", str(CAMERA_FOV))
+
+ # 生成相机（附加到车辆）
+ self.camera = self.world.spawn_actor(camera_bp, CAMERA_POS, attach_to=self.vehicle)
+
+ # 注册图像回调函数
+ self.camera.listen(self._on_image)
+
+ def _on_image(self, image):
+ # 将CARLA图像转换为Pygame Surface
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((image.height, image.width, 4))
+ array = array[:, :, :3]
+ array = array[:, :, ::-1]
+ array = np.swapaxes(array, 0, 1)
+
+ # 存储为Pygame Surface
+ self.image_surface = pygame.surfarray.make_surface(array)
+
+ def destroy(self):
+ if self.camera:
+ self.camera.stop()
+ self.camera.destroy()
+
+# ===================== 车辆控制类 ======================
+class VehicleAgent:
+ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_speed):
+ self.vehicle_id = vehicle_id
+ self.world = world
+ self.map = map
+ self.base_speed = base_speed
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+
+ # 生成车辆
+ self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
+ if self.vehicle_bp.has_attribute("color"):
+ color = random.choice(self.vehicle_bp.get_attribute("color").recommended_values)
+ self.vehicle_bp.set_attribute("color", color)
+
+ self.vehicle = self._spawn_vehicle_with_retry(spawn_point)
+ if not self.vehicle:
+ raise RuntimeError(f"车辆{vehicle_id}生成失败")
+
+ # 创建相机
+ self.camera = VehicleCamera(world, self.vehicle, vehicle_id)
+
+ # 初始化控制器
+ self.pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ self.speed_controller = SpeedController(PID_KP, PID_KI, PID_KD, base_speed)
+ self.traffic_light_manager = TrafficLightManager(vehicle_id)
+
+ self.is_alive = True
+ self.logger.info(f"生成成功，车型：{vehicle_model}，出生点：({spawn_point.location.x:.1f},{spawn_point.location.y:.1f})")
+
+ def _spawn_vehicle_with_retry(self, initial_spawn_point):
+ all_spawn_points = self.map.get_spawn_points()
+ if not all_spawn_points:
+ self.logger.error("地图中无有效出生点")
+ return None
+
+ candidate_points = [initial_spawn_point]
+ candidate_points += random.sample(all_spawn_points, min(10, len(all_spawn_points)))
+
+ for retry in range(SPAWN_RETRY_MAX):
+ spawn_point = candidate_points[retry % len(candidate_points)]
+ spawn_point.location.z += 0.3
+ spawn_point.rotation.yaw += random.randint(-5, 5)
+
+ if not check_spawn_collision(self.world, spawn_point):
+ self.logger.warning(f"第{retry+1}次重试：出生点有碰撞风险，跳过")
+ time.sleep(SPAWN_RETRY_DELAY)
+ continue
+
+ try:
+ return self.world.spawn_actor(self.vehicle_bp, spawn_point)
+ except Exception as e:
+ self.logger.warning(f"第{retry+1}次重试失败：{e}")
+ time.sleep(SPAWN_RETRY_DELAY)
+
+ self.logger.error(f"超过{SPAWN_RETRY_MAX}次重试，生成失败")
+ return None
+
+ def update(self):
+ if not self.is_alive or not is_actor_alive(self.vehicle):
+ self.is_alive = False
+ self.logger.error("车辆已销毁，停止更新")
+ return False
+
+ try:
+ # 获取车辆状态
+ vehicle_transform = self.vehicle.get_transform()
+ vehicle_vel = self.vehicle.get_velocity()
+ current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6
+
+ # 路径跟踪
+ current_wp = get_forward_waypoint(self.vehicle, self.map)
+ dir_change, curve_level = calculate_dir_change(current_wp)
+ lookahead_dist = self.pp_controller.get_adaptive_lookahead(dir_change)
+ target_wps = current_wp.next(lookahead_dist)
+ target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+
+ # 速度控制
+ curve_speed_factors = [1.0, 0.7, 0.4]
+ speed_factor = curve_speed_factors[min(curve_level, 2)]
+ base_target_speed = self.base_speed * speed_factor
+ base_target_speed = max(8.0, base_target_speed)
+
+ # 跟车控制
+ global vehicle_agents
+ if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
try:
- tl.set_state(carla.TrafficLightState.Yellow)
+ lead_vehicle = vehicle_agents[self.vehicle_id - 2].vehicle
+ lead_vehicle_transform = lead_vehicle.get_transform()
+ dist_to_lead = vehicle_transform.location.distance(lead_vehicle_transform.location)
+ if dist_to_lead < 15.0:
+ base_target_speed = max(5.0, base_target_speed * 0.5)
except:
pass
- time.sleep(YELLOW_LIGHT_DURATION)
-# ===================== 纯追踪控制器 =====================
+ # 交通灯处理
+ target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
+ self.vehicle, current_speed, base_target_speed
+ )
+
+ # 计算控制指令
+ steer = self.pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
+ throttle = self.speed_controller.calculate(target_speed, current_speed)
+ brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
+
+ if "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
+ throttle = 0.0
+ brake = 1.0
+
+ # 应用控制
+ control = carla.VehicleControl()
+ control.steer = steer
+ control.throttle = throttle
+ control.brake = brake
+ self.vehicle.apply_control(control)
+
+ # 日志输出
+ self.logger.info(
+ f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | "
+ f"弯道：{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态：{traffic_light_status}"
+ )
+
+ return True
+
+ except Exception as e:
+ self.logger.error(f"更新失败：{e}", exc_info=True)
+ return False
+
+ def destroy(self):
+ # 销毁相机
+ self.camera.destroy()
+ # 销毁车辆
+ if self.vehicle and is_actor_alive(self.vehicle):
+ self.vehicle.destroy()
+ self.logger.info("车辆资源已清理")
+
+# ===================== 控制器类 ======================
class AdaptivePurePursuit:
def __init__(self, wheelbase):
self.wheelbase = wheelbase
@@ -158,7 +461,6 @@ def __init__(self, wheelbase):
self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT
def calculate_steer(self, vehicle_transform, target_point, dir_change):
- # 1. 后轴位置
forward_vec = vehicle_transform.get_forward_vector()
rear_axle_loc = carla.Location(
x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
@@ -166,7 +468,6 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
z=vehicle_transform.location.z
)
- # 2. 车辆坐标系转换
dx = target_point.x - rear_axle_loc.x
dy = target_point.y - rear_axle_loc.y
yaw = math.radians(vehicle_transform.rotation.yaw)
@@ -174,7 +475,6 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
- # 3. 转向增益
steer_gain = np.interp(
dir_change,
[0, DIR_CHANGE_SHARP],
@@ -182,15 +482,13 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
)
steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
- # 4. 纯追踪计算
if dx_vehicle < 0.1:
steer = self.last_steer
else:
- steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * LANE_KEEP_STRICTNESS, dx_vehicle ** 2 + dy_vehicle ** 2)
+ steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle, dx_vehicle ** 2 + dy_vehicle ** 2)
steer = steer_rad / math.pi
steer *= steer_gain
- # 5. 死区+滤波
if abs(steer) < STEER_DEADZONE:
steer = 0.0
steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
@@ -209,12 +507,12 @@ def get_adaptive_lookahead(self, dir_change):
self.last_lookahead = lookahead_dist
return lookahead_dist
-# ===================== 速度控制器 =====================
class SpeedController:
- def __init__(self, kp, ki, kd):
+ def __init__(self, kp, ki, kd, base_speed):
self.kp = kp
self.ki = ki
self.kd = kd
+ self.base_speed = base_speed
self.last_error = 0.0
self.integral = 0.0
@@ -228,15 +526,16 @@ def calculate(self, target_speed, current_speed):
self.last_error = error
return np.clip(p + i + d, 0.0, 1.0)
-# ===================== 交通灯管理类 ======================
class TrafficLightManager:
- def __init__(self):
+ def __init__(self, vehicle_id):
+ self.vehicle_id = vehicle_id
self.tracked_light = None
self.is_stopped_at_red = False
self.red_light_stop_time = 0
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_transform):
- """计算车辆前进方向与交通灯的夹角"""
vehicle_forward = vehicle_transform.get_forward_vector()
vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y])
vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward)
@@ -252,18 +551,15 @@ def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_tr
return angle
def get_lane_traffic_light(self, vehicle, world):
- """扩大检测范围，检测更多交通灯"""
vehicle_transform = vehicle.get_transform()
vehicle_loc = vehicle_transform.location
- # 检查跟踪的交通灯是否存活
if self.tracked_light and is_actor_alive(self.tracked_light):
dist = self.tracked_light.get_transform().location.distance(vehicle_loc)
angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, self.tracked_light.get_transform())
if dist < TRAFFIC_LIGHT_DETECTION_RANGE and angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
return self.tracked_light
- # 获取所有交通灯并筛选有效灯
traffic_lights = world.get_actors().filter("traffic.traffic_light")
valid_lights = []
@@ -286,16 +582,14 @@ def get_lane_traffic_light(self, vehicle, world):
return None
def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
- """红灯强制停车，绿灯恢复行驶"""
world = vehicle.get_world()
traffic_light = self.get_lane_traffic_light(vehicle, world)
if not traffic_light:
self.is_stopped_at_red = False
self.red_light_stop_time = 0
- return base_target_speed, "No Light (Lane)"
+ return base_target_speed, "No Light"
- # 核心：使用兼容函数获取停止线位置
stop_line_loc = get_traffic_light_stop_line(traffic_light)
dist_to_stop_line = vehicle.get_transform().location.distance(stop_line_loc)
@@ -305,296 +599,310 @@ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
target_speed = max(STOP_SPEED_THRESHOLD, recovery_speed)
if abs(target_speed - base_target_speed) < 0.5:
self.is_stopped_at_red = False
- return target_speed, "Green (Recovering)"
- return base_target_speed, "Green (Lane)"
+ self.logger.info(f"绿灯恢复行驶，目标速度：{target_speed:.1f}km/h")
+ return target_speed, "Green"
+ return base_target_speed, "Green"
elif traffic_light.get_state() == carla.TrafficLightState.Yellow:
self.is_stopped_at_red = False
yellow_speed = max(5.0, base_target_speed * 0.3)
- return yellow_speed, "Yellow (Stop Soon)"
+ self.logger.warning(f"黄灯减速，目标速度：{yellow_speed:.1f}km/h")
+ return yellow_speed, "Yellow"
elif traffic_light.get_state() == carla.TrafficLightState.Red:
if dist_to_stop_line > TRAFFIC_LIGHT_STOP_DISTANCE:
self.is_stopped_at_red = False
red_speed = max(2.0, current_speed * 0.1)
- return red_speed, f"Red (Decelerating: {dist_to_stop_line:.1f}m)"
+ self.logger.warning(f"红灯减速，距离停止线：{dist_to_stop_line:.1f}m，目标速度：{red_speed:.1f}km/h")
+ return red_speed, "Red"
else:
if current_speed <= STOP_SPEED_THRESHOLD:
self.is_stopped_at_red = True
self.red_light_stop_time += 1
wait_seconds = self.red_light_stop_time // 30
- return 0.0, f"Red (Stopped: {wait_seconds}s)"
+ self.logger.info(f"红灯停车等待：{wait_seconds}s")
+ return 0.0, "Red (Stopped)"
else:
- return 0.0, "Red (Emergency Stop)"
+ self.logger.warning("红灯紧急制动")
+ return 0.0, "Red (Braking)"
- return base_target_speed, "Unknown Light"
+ return base_target_speed, "Unknown"
-# ===================== 车道行驶辅助函数 ======================
-def calculate_dir_change(current_wp):
- """计算方向变化量，判断弯道等级"""
- waypoints = [current_wp]
- for i in range(5):
- next_wps = waypoints[-1].next(1.0)
- if next_wps:
- waypoints.append(next_wps[0])
- else:
- break
-
- if len(waypoints) < 4:
- return 0.0, 0
-
- dirs = []
- for i in range(1, len(waypoints)):
- wp_prev = waypoints[i-1]
- wp_curr = waypoints[i]
- dir_rad = math.atan2(
- wp_curr.transform.location.y - wp_prev.transform.location.y,
- wp_curr.transform.location.x - wp_prev.transform.location.x
- )
- dirs.append(dir_rad)
-
- dir_change = 0.0
- for i in range(1, len(dirs)):
- dir_change += abs(dirs[i] - dirs[i-1]) * 2
-
- if dir_change < DIR_CHANGE_GENTLE:
- curve_level = 0
- elif dir_change < DIR_CHANGE_SHARP:
- curve_level = 1
- else:
- curve_level = 2
-
- return dir_change, curve_level
-
-def get_forward_waypoint(vehicle, map):
- """获取车辆当前车道的前进方向路点（极低版本兼容）"""
- vehicle_transform = vehicle.get_transform()
- # 1. 投影到道路
- current_wp = map.get_waypoint(
- vehicle_transform.location,
- project_to_road=True
- )
-
- # 2. 纯数学判断方向是否相反
- road_direction = current_wp.transform.get_forward_vector()
- vehicle_direction = vehicle_transform.get_forward_vector()
- dot_product = road_direction.x * vehicle_direction.x + road_direction.y * vehicle_direction.y
-
- # 3. 方向相反则取前方点
- if dot_product < ROAD_DIRECTION_DOT_THRESHOLD:
- forward_wps = current_wp.next(10.0)
- if forward_wps:
- current_wp = forward_wps[0]
- else:
- current_wp = map.get_waypoint(
- vehicle_transform.location + vehicle_direction * 5.0,
- project_to_road=True
- )
-
- return current_wp
-
-# ===================== 相机管理器 =====================
-class CameraManager:
- def __init__(self, world, vehicle, display):
- self.world = world
- self.vehicle = vehicle
- self.display = display
- self.camera = None
- self.traffic_light_status = "No Light"
- self._create_camera()
-
- def _create_camera(self):
- bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
- bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
- bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
- bp.set_attribute("fov", str(CAMERA_FOV))
- self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
- self.camera.listen(self._on_image)
+# ===================== 交通灯控制线程 ======================
+def cycle_traffic_light_states(world, stop_event):
+ logger = logging.getLogger(__name__)
+ logger = logging.LoggerAdapter(logger, {"vehicle_id": "系统"})
+ while not stop_event.is_set():
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ if not traffic_lights:
+ time.sleep(1)
+ continue
- def _on_image(self, image):
- """修正数组处理逻辑"""
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))
- array = array[:, :, :3]
- array = array[:, :, ::-1].swapaxes(0, 1)
- self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
- self._draw_info()
- pygame.display.flip()
-
- def _draw_info(self):
- font = pygame.font.SysFont("Arial", 24, bold=True)
- if "Red" in self.traffic_light_status:
- color = (255, 0, 0)
- elif "Green" in self.traffic_light_status:
- color = (0, 255, 0)
- elif "Yellow" in self.traffic_light_status:
- color = (255, 255, 0)
- else:
- color = (255, 255, 255)
+ # 红灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Red)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为红灯，持续{RED_LIGHT_DURATION}秒")
+ stop_event.wait(RED_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
- text = font.render(f"Traffic Light: {self.traffic_light_status}", True, color)
- bg = pygame.Surface((text.get_width() + 10, text.get_height() + 5))
- bg.fill((0, 0, 0))
- self.display.blit(bg, (5, 5))
- self.display.blit(text, (10, 7))
+ # 绿灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Green)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为绿灯，持续{GREEN_LIGHT_DURATION}秒")
+ stop_event.wait(GREEN_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
- def update_traffic_light_status(self, status):
- self.traffic_light_status = status
+ # 黄灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Yellow)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为黄灯，持续{YELLOW_LIGHT_DURATION}秒")
+ stop_event.wait(YELLOW_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
- def destroy(self):
- if self.camera:
- if is_actor_alive(self.camera):
- self.camera.stop()
- self.camera.destroy()
+ logger.info("交通灯线程停止")
-# ===================== 主函数（核心：多交通灯配置）=====================
+# ===================== 主函数 ======================
def main():
+ global current_view_vehicle_id, vehicle_agents
pygame.init()
- display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
- pygame.display.set_caption("CARLA 多交通灯版（Town04密集交通灯+状态循环）")
+ screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
+ pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
client = None
world = None
- vehicle = None
- camera_manager = None
- pp_controller = None
- speed_controller = None
- traffic_light_manager = None
+ map = None
+ tl_cycle_thread = None
+ tl_stop_event = threading.Event()
+ show_split_screen = True
+ show_top_view = False
+ top_view_camera = None
+
+ # 清理函数
+ def cleanup():
+ print("\n开始清理资源...")
+ tl_stop_event.set()
+ if tl_cycle_thread and tl_cycle_thread.is_alive():
+ tl_cycle_thread.join(timeout=2)
+
+ if top_view_camera:
+ top_view_camera.stop()
+ top_view_camera.destroy()
+
+ for agent in vehicle_agents:
+ agent.destroy()
+
+ if world:
+ for actor in world.get_actors():
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
+ if is_actor_alive(actor):
+ actor.destroy()
+
+ pygame.quit()
+ print("资源清理完成")
+
+ # 注册退出回调
+ import atexit
+ import signal
+ atexit.register(cleanup)
+ signal.signal(signal.SIGINT, lambda sig, frame: sys.exit(0))
try:
- # 1. 连接CARLA，加载**Town04**（交通灯最密集的地图）
+ # 连接CARLA
client = carla.Client(CARLA_HOST, CARLA_PORT)
client.set_timeout(CARLA_TIMEOUT)
try:
- client.load_world("Town04") # 替换为Town04，交通灯数量远多于Town03
- except:
+ world = client.load_world("Town04")
+ print("成功加载Town04地图")
+ except Exception as e:
world = client.get_world()
- print("警告：Town04地图不存在，使用当前地图")
- else:
- world = client.get_world()
- print("成功加载Town04地图（交通灯密集）")
+ print(f"警告：Town04地图加载失败（{e}），使用当前地图")
map = world.get_map()
- # 2. 清理残留演员
+ # 清理残留演员
+ print("清理残留演员...")
for actor in world.get_actors():
- if actor.type_id.startswith(("vehicle.", "walker.", "sensor.", "controller.")):
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
if is_actor_alive(actor):
actor.destroy()
- print("残留演员清理完成")
-
- # 3. 启动交通灯状态循环线程（后台持续切换交通灯状态）
- import threading
- tl_cycle_thread = threading.Thread(target=cycle_traffic_light_states, args=(world,), daemon=True)
- tl_cycle_thread.start()
- print("交通灯状态循环线程已启动（红3秒→绿5秒→黄2秒）")
-
- # 4. 设置车辆起始位置（Town04交通灯密集区）
- vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
-
- # 手动指定Town04的核心交通灯路口坐标（经测试：多个交通灯环绕）
- spawn_point = carla.Transform(
- carla.Location(x=220.0, y=150.0, z=0.5), # Town04核心交通灯密集区
- carla.Rotation(yaw=90.0)
- )
- spawn_point.location.z += 0.2
- vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
-
- # 备用方案：自动查找交通灯附近的出生点
- if not vehicle:
- print("手动坐标生成失败，自动查找交通灯附近的出生点...")
- spawn_point = get_spawn_point_near_traffic_light(world, map)
- vehicle = world.spawn_actor(vehicle_bp, spawn_point)
-
- # 最终备用方案
- if not vehicle:
- spawn_points = map.get_spawn_points()
- spawn_point = spawn_points[0] if spawn_points else carla.Transform()
- vehicle = world.spawn_actor(vehicle_bp, spawn_point)
-
- print(f"车辆生成成功：{vehicle.type_id}（起始于交通灯密集区）")
-
- # 5. 初始化组件
- pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
- speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
- traffic_light_manager = TrafficLightManager()
- camera_manager = CameraManager(world, vehicle, display)
-
- # 6. 主循环
- print("仿真启动，按ESC退出...（车辆将经过多个交通灯）")
- clock = pygame.time.Clock()
- running = True
-
- while running:
- for event in pygame.event.get():
- if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
- running = False
+ time.sleep(3.0)
+ print("清理完成")
+
+ # 自动获取地图的第一个出生点作为基准（避免手动坐标无效）
+ base_location = None
+ all_spawn_points = map.get_spawn_points()
+ if all_spawn_points:
+ base_location = all_spawn_points[0].location
+ print(f"使用地图第一个出生点作为基准：({base_location.x:.1f}, {base_location.y:.1f})")
+ else:
+ base_location = carla.Location(x=220.0, y=150.0, z=0.5)
- # 获取车辆状态
- vehicle_transform = vehicle.get_transform()
- vehicle_vel = vehicle.get_velocity()
- current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6
+ # 获取有效的出生点
+ print(f"获取{VEHICLE_COUNT}个有效出生点...")
+ valid_spawn_points = get_valid_spawn_points(map, VEHICLE_COUNT, base_location)
+ for i, sp in enumerate(valid_spawn_points):
+ print(f" 出生点{i+1}：({sp.location.x:.1f},{sp.location.y:.1f})")
- # 核心：获取前进方向路点
- current_wp = get_forward_waypoint(vehicle, map)
+ # 生成车辆
+ print(f"\n分步生成车辆（间隔{SPAWN_INTERVAL}秒）...")
+ for i in range(VEHICLE_COUNT):
+ vehicle_model = VEHICLE_MODELS[i % len(VEHICLE_MODELS)]
+ base_speed = BASE_SPEEDS[i % len(BASE_SPEEDS)]
+ spawn_point = valid_spawn_points[i]
- # 计算方向变化
- dir_change, curve_level = calculate_dir_change(current_wp)
+ try:
+ print(f"\n生成车辆{i+1}（车型：{vehicle_model}）...")
+ agent = VehicleAgent(world, map, i+1, spawn_point, vehicle_model, base_speed)
+ vehicle_agents.append(agent)
+ print(f"车辆{i+1}生成成功！")
+ except Exception as e:
+ print(f"车辆{i+1}生成失败：{e}")
- # 预瞄距离
- lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)
+ time.sleep(SPAWN_INTERVAL)
- # 目标路点
- target_wps = current_wp.next(lookahead_dist)
- target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+ if len(vehicle_agents) == 0:
+ raise RuntimeError("无车辆生成成功，仿真终止")
- # 基础速度
- curve_speed_factors = [1.0, 0.7, 0.4]
- speed_factor = curve_speed_factors[min(curve_level, 2)]
- base_target_speed = BASE_SPEED * speed_factor
- base_target_speed = max(8.0, base_target_speed)
+ print(f"\n共生成{len(vehicle_agents)}辆车辆！")
- # 交通灯处理（检测多个交通灯）
- target_speed, traffic_light_status = traffic_light_manager.handle_traffic_light_logic(
- vehicle, current_speed, base_target_speed
+ # 创建全局俯视相机
+ try:
+ top_view_bp = world.get_blueprint_library().find("sensor.camera.rgb")
+ top_view_bp.set_attribute("image_size_x", str(WINDOW_WIDTH))
+ top_view_bp.set_attribute("image_size_y", str(WINDOW_HEIGHT))
+ top_view_bp.set_attribute("fov", str(90))
+ top_view_transform = carla.Transform(
+ vehicle_agents[0].vehicle.get_transform().location + carla.Location(z=50),
+ carla.Rotation(pitch=-90)
)
- camera_manager.update_traffic_light_status(traffic_light_status)
-
- # 控制计算
- steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
- throttle = speed_controller.calculate(target_speed, current_speed)
- brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
-
- # 红灯刹车
- if "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
- throttle = 0.0
- brake = 1.0
+ top_view_camera = world.spawn_actor(top_view_bp, top_view_transform)
+ top_view_surface = None
+ top_view_camera.listen(lambda image: globals().update({
+ "top_view_surface": pygame.surfarray.make_surface(
+ np.swapaxes(np.array(image.raw_data).reshape((image.height, image.width, 4))[:, :, :3][:, :, ::-1], 0, 1)
+ )
+ }))
+ except:
+ print("警告：无法创建俯视相机")
- # 应用控制
- control = carla.VehicleControl()
- control.steer = steer
- control.throttle = throttle
- control.brake = brake
- vehicle.apply_control(control)
+ # 启动交通灯线程
+ tl_cycle_thread = threading.Thread(target=cycle_traffic_light_states, args=(world, tl_stop_event), daemon=True)
+ tl_cycle_thread.start()
+ print("交通灯线程启动")
- # 打印状态（显示当前交通灯状态）
- curve_names = ["直道", "缓弯", "急弯"]
- lane_id = current_wp.lane_id
- print(f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | 弯道：{curve_names[curve_level]:<3} | 车道ID：{lane_id} | 灯状态：{traffic_light_status}")
+ # 主循环
+ clock = pygame.time.Clock()
+ running = True
+ while running:
+ # 事件处理
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ running = False
+ elif event.type == pygame.KEYDOWN:
+ if event.key == pygame.K_1 and len(vehicle_agents) >= 1:
+ current_view_vehicle_id = 1
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_2 and len(vehicle_agents) >= 2:
+ current_view_vehicle_id = 2
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_3 and len(vehicle_agents) >= 3:
+ current_view_vehicle_id = 3
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_s:
+ show_split_screen = True
+ show_top_view = False
+ elif event.key == pygame.K_v:
+ show_top_view = True
+ show_split_screen = False
+ elif event.key == pygame.K_ESCAPE:
+ running = False
+
+ # 清空屏幕
+ screen.fill((0, 0, 0))
+
+ if show_top_view:
+ if top_view_surface:
+ screen.blit(top_view_surface, (0, 0))
+ elif show_split_screen:
+ if len(vehicle_agents) == 1:
+ agent = vehicle_agents[0]
+ if agent.camera.image_surface:
+ surface = pygame.transform.scale(agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surface, (0, 0))
+ elif len(vehicle_agents) == 2:
+ agent1 = vehicle_agents[0]
+ agent2 = vehicle_agents[1]
+
+ if agent1.camera.image_surface:
+ surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ screen.blit(surface1, (0, 0))
+
+ if agent2.camera.image_surface:
+ surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ screen.blit(surface2, (WINDOW_WIDTH//2, 0))
+ elif len(vehicle_agents) >= 3:
+ agent1 = vehicle_agents[0]
+ agent2 = vehicle_agents[1]
+ agent3 = vehicle_agents[2]
+
+ if agent1.camera.image_surface:
+ surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surface1, (0, 0))
+
+ if agent2.camera.image_surface:
+ surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surface2, (WINDOW_WIDTH//2, 0))
+
+ if agent3.camera.image_surface:
+ surface3 = pygame.transform.scale(agent3.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
+ screen.blit(surface3, (0, WINDOW_HEIGHT//2))
+ else:
+ target_agent = None
+ for agent in vehicle_agents:
+ if agent.vehicle_id == current_view_vehicle_id:
+ target_agent = agent
+ break
+
+ if target_agent and target_agent.camera.image_surface:
+ surface = pygame.transform.scale(target_agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surface, (0, 0))
+
+ # 更新车辆状态
+ with ThreadPoolExecutor(max_workers=VEHICLE_COUNT) as executor:
+ futures = [executor.submit(agent.update) for agent in vehicle_agents]
+ for future in as_completed(futures):
+ try:
+ future.result()
+ except Exception as e:
+ print(f"车辆更新异常：{e}")
+
+ # 刷新屏幕
+ pygame.display.flip()
clock.tick(30)
except Exception as e:
- print(f"错误：{e}")
+ print(f"仿真异常：{e}")
traceback.print_exc()
-
finally:
- print("清理资源...")
- if camera_manager:
- camera_manager.destroy()
- if vehicle and is_actor_alive(vehicle):
- vehicle.destroy()
- pygame.quit()
- print("仿真结束")
+ cleanup()
if __name__ == "__main__":
main()
\ No newline at end of file
From 63165f8b86d1ae9ffe4ff84ef4f14424a5e79c33 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sun, 21 Dec 2025 20:35:27 +0800
Subject: [PATCH 15/26] =?UTF-8?q?=E6=9B=B4=E6=96=B0README.md?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/README.md | 95 +++++++++++++++++++++++++++-
1 file changed, 94 insertions(+), 1 deletion(-)
diff --git a/src/autonomous_driving_car/README.md b/src/autonomous_driving_car/README.md
index cffa4a791f..f98897cbad 100644
--- a/src/autonomous_driving_car/README.md
+++ b/src/autonomous_driving_car/README.md
@@ -1 +1,94 @@
-CARLA 模拟器和自动驾驶基础算法学习
\ No newline at end of file
+CARLA 模拟器和自动驾驶基础算法学习
+
+
+
+\# 无人驾驶汽车项目（基于CARLA模拟器）
+
+\## 项目简介
+
+本项目是基于CARLA开源仿真平台、Python和PyCharm开发的无人驾驶仿真系统，融合计算机视觉、路径规划与控制技术，实现虚拟场景中车辆的自主导航、避障与路径跟踪功能，适用于自动驾驶入门实践。
+
+
+
+\## 核心功能
+
+\- 路径规划：A\*算法、RRT/RRT\*算法实现起点到终点路径生成
+
+\- 障碍物检测：YOLOv8+OpenCV实时识别目标，激光雷达点云聚类定位
+
+\- 车辆控制：PID控制器实现转向、速度精准控制
+
+\- CARLA交互：加载场景、获取传感器数据（摄像头/激光雷达等）、发送控制指令
+
+\- 实时可视化：PyGame显示场景、车辆状态与检测结果
+
+
+
+\## 技术栈
+
+| 类别 | 具体技术/工具 |
+
+|--------------|---------------------------------------|
+
+| 开发环境 | PyCharm Community Edition 2024+、Windows/Linux |
+
+| 核心语言 | Python 3.8+ |
+
+| 仿真平台 | CARLA 0.9.15/0.9.16、CARLA Python API |
+
+| 计算机视觉 | OpenCV、NumPy、YOLOv8（Ultralytics） |
+
+| 路径规划 | A\*、RRT/RRT\*算法 |
+
+| 控制理论 | PID控制器 |
+
+| 可视化与数据 | PyGame、Matplotlib、Pandas |
+
+
+
+\## 快速开始
+
+1\. 克隆项目到PyCharm，创建Python 3.8+虚拟环境
+
+2\. 安装依赖：`pip install -r requirements.txt`（核心依赖：carla、opencv-python、ultralytics、numpy、pygame）
+
+3\. 启动CARLA模拟器（运行`CarlaUE4.exe`/`CarlaUE4.sh`）
+
+4\. 运行主程序：`python main.py`，自动连接CARLA并启动自主导航
+
+
+
+\## 项目结构
+
+```
+
+self-driving-car-carla/
+
+├── carla\_client/ # CARLA连接与场景管理
+
+├── perception/ # 目标检测与传感器数据处理
+
+├── planning/ # 路径规划算法实现
+
+├── control/ # PID控制逻辑
+
+├── visualization/ # 实时可视化
+
+├── utils/ # 工具函数
+
+├── main.py # 项目入口
+
+└── requirements.txt # 依赖列表
+
+```
+
+
+
+\## 常见问题
+
+\- CARLA连接失败：确保模拟器已启动，Python API版本与CARLA一致
+
+\- 检测速度慢：使用YOLOv8n轻量模型，或启用GPU加速
+
+\- 控制不稳定：调整PID参数或增加路径平滑处理
+
From 38a6d8b0803746feda9744f5e33cd592da177e9a Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sun, 21 Dec 2025 21:30:38 +0800
Subject: [PATCH 16/26] =?UTF-8?q?=E6=9B=B4=E6=96=B0README.md?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/README.md | 26 +++++---------------------
1 file changed, 5 insertions(+), 21 deletions(-)
diff --git a/src/autonomous_driving_car/README.md b/src/autonomous_driving_car/README.md
index f98897cbad..8125279229 100644
--- a/src/autonomous_driving_car/README.md
+++ b/src/autonomous_driving_car/README.md
@@ -60,27 +60,11 @@ CARLA 模拟器和自动驾驶基础算法学习
\## 项目结构
-```
-
-self-driving-car-carla/
-
-├── carla\_client/ # CARLA连接与场景管理
-
-├── perception/ # 目标检测与传感器数据处理
-
-├── planning/ # 路径规划算法实现
-
-├── control/ # PID控制逻辑
-
-├── visualization/ # 实时可视化
-
-├── utils/ # 工具函数
-
-├── main.py # 项目入口
-
-└── requirements.txt # 依赖列表
-
-```
+autonomous_driving_car/
+├── FPV/ # 第一视角可视化模块：负责车辆摄像头视角、检测结果的实时显示
+├── MCP/ # 主控制与模块集成：包含感知（检测）、规划（路径）、控制（PID）的核心逻辑
+├── main.py # 项目入口：启动CARLA连接、调用MCP与FPV模块
+└── README.md # 项目说明文档
From 4c71825a29dbfec835caf2fd2cb2e139fe76eadd Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Sun, 21 Dec 2025 21:38:54 +0800
Subject: [PATCH 17/26] =?UTF-8?q?=E6=9B=B4=E6=96=B0README.md?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/README.md | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/autonomous_driving_car/README.md b/src/autonomous_driving_car/README.md
index 8125279229..0bb5ae9ee2 100644
--- a/src/autonomous_driving_car/README.md
+++ b/src/autonomous_driving_car/README.md
@@ -54,7 +54,7 @@ CARLA 模拟器和自动驾驶基础算法学习
3\. 启动CARLA模拟器（运行`CarlaUE4.exe`/`CarlaUE4.sh`）
-4\. 运行主程序：`python main.py`，自动连接CARLA并启动自主导航
+4\. 运行主程序：`python main.py`，自动连接CARLA并自主导航
From 17f2b0cc3d8f1076732245923719eb83b51583d9 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Mon, 22 Dec 2025 09:34:34 +0800
Subject: [PATCH 18/26] =?UTF-8?q?=E8=BF=9B=E8=A1=8Cros=E5=B0=81=E8=A3=85?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../ros/carla_multi_vehicle_node.py | 938 ++++++++++++++++++
.../ros/carla_vehicle_control.py | 908 +++++++++++++++++
2 files changed, 1846 insertions(+)
create mode 100644 src/autonomous_driving_car/ros/carla_multi_vehicle_node.py
create mode 100644 src/autonomous_driving_car/ros/carla_vehicle_control.py
diff --git a/src/autonomous_driving_car/ros/carla_multi_vehicle_node.py b/src/autonomous_driving_car/ros/carla_multi_vehicle_node.py
new file mode 100644
index 0000000000..8fa2fd0842
--- /dev/null
+++ b/src/autonomous_driving_car/ros/carla_multi_vehicle_node.py
@@ -0,0 +1,938 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+CARLA多车辆协同控制的ROS节点版
+"""
+# ===================== ROS核心导入（新增）======================
+import rospy
+from std_msgs.msg import String # 用于发布车辆状态的标准消息
+# ==============================================================
+
+import sys
+import os
+import carla
+import numpy as np
+import math
+import pygame
+import traceback
+import time
+import threading
+from concurrent.futures import ThreadPoolExecutor, as_completed
+import logging
+import random
+
+# ===================== 全局配置 =======================
+# CARLA连接（必须修改为你的主机IP）
+CARLA_HOST = "192.168.137.112"
+CARLA_PORT = 2000
+CARLA_TIMEOUT = 30.0 # 超时时间延长到30秒，提高容错
+
+# ROS配置（新增）
+ROS_NODE_NAME = "carla_multi_vehicle_node" # ROS节点名称
+VEHICLE_STATUS_TOPIC = "/carla/vehicle_status" # 发布车辆状态的话题名
+
+# 多车辆配置
+VEHICLE_COUNT = 3
+VEHICLE_MODELS = [
+ "vehicle.tesla.model3",
+ "vehicle.bmw.grandtourer",
+ "vehicle.audi.a2"
+]
+SPAWN_INTERVAL = 1.0
+SPAWN_RETRY_MAX = 8
+SPAWN_RETRY_DELAY = 0.5
+SPAWN_DISTANCE_LIMIT = 15.0 # 放宽距离限制到15米
+
+# 车辆控制参数
+VEHICLE_WHEELBASE = 2.9
+VEHICLE_REAR_AXLE_OFFSET = 1.45
+LOOKAHEAD_DIST_STRAIGHT = 7.0
+LOOKAHEAD_DIST_CURVE = 4.0
+STEER_GAIN_STRAIGHT = 0.7
+STEER_GAIN_CURVE = 1.0
+STEER_DEADZONE = 0.05
+STEER_LOWPASS_ALPHA = 0.6
+MAX_STEER = 1.0
+DIR_CHANGE_GENTLE = 0.03
+DIR_CHANGE_SHARP = 0.08
+BASE_SPEEDS = [25.0, 22.0, 20.0]
+PID_KP = 0.2
+PID_KI = 0.01
+PID_KD = 0.02
+
+# 交通规则配置
+TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
+TRAFFIC_LIGHT_DETECTION_RANGE = 50.0
+TRAFFIC_LIGHT_ANGLE_THRESHOLD = 60.0
+STOP_SPEED_THRESHOLD = 0.2
+GREEN_LIGHT_ACCEL_FACTOR = 0.25
+STOP_LINE_SIM_DISTANCE = 5.0
+RED_LIGHT_DURATION = 3.0
+GREEN_LIGHT_DURATION = 5.0
+YELLOW_LIGHT_DURATION = 2.0
+
+# 相机配置
+WINDOW_WIDTH = 1280
+WINDOW_HEIGHT = 720
+CAMERA_FOV = 120
+CAMERA_POS = carla.Transform(carla.Location(x=-6.0, z=2.5), carla.Rotation(pitch=-5))
+
+# 全局变量
+current_view_vehicle_id = 1
+vehicle_agents = []
+
+# 日志配置
+logging.basicConfig(
+ level=logging.INFO,
+ format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
+ handlers=[logging.FileHandler("multi_vehicle_simulation.log"), logging.StreamHandler()]
+)
+
+# ===================== 核心工具函数 ======================
+def is_actor_alive(actor):
+ try:
+ return actor.is_alive()
+ except TypeError:
+ return actor.is_alive
+
+def get_traffic_light_stop_line(traffic_light):
+ try:
+ return traffic_light.get_stop_line_location()
+ except AttributeError:
+ tl_transform = traffic_light.get_transform()
+ forward_vec = tl_transform.get_forward_vector()
+ stop_line_loc = tl_transform.location - forward_vec * STOP_LINE_SIM_DISTANCE
+ stop_line_loc.z = tl_transform.location.z
+ return stop_line_loc
+
+def calculate_dir_change(current_wp):
+ waypoints = [current_wp]
+ for i in range(5):
+ next_wps = waypoints[-1].next(1.0)
+ if next_wps:
+ waypoints.append(next_wps[0])
+ else:
+ break
+
+ if len(waypoints) < 4:
+ return 0.0, 0
+
+ dirs = []
+ for i in range(1, len(waypoints)):
+ wp_prev = waypoints[i-1]
+ wp_curr = waypoints[i]
+ dir_rad = math.atan2(
+ wp_curr.transform.location.y - wp_prev.transform.location.y,
+ wp_curr.transform.location.x - wp_prev.transform.location.x
+ )
+ dirs.append(dir_rad)
+
+ dir_change = 0.0
+ for i in range(1, len(dirs)):
+ dir_change += abs(dirs[i] - dirs[i-1]) * 2
+
+ if dir_change < DIR_CHANGE_GENTLE:
+ curve_level = 0
+ elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 1
+ else:
+ curve_level = 2
+
+ return dir_change, curve_level
+
+def get_forward_waypoint(vehicle, map):
+ vehicle_transform = vehicle.get_transform()
+ current_wp = map.get_waypoint(
+ vehicle_transform.location,
+ project_to_road=True,
+ lane_type=carla.LaneType.Driving
+ )
+
+ # 所有车辆使用同一车道的路点
+ global vehicle_agents
+ if len(vehicle_agents) > 0:
+ try:
+ lead_vehicle_wp = map.get_waypoint(vehicle_agents[0].vehicle.get_transform().location, project_to_road=True)
+ current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True, lane_id=lead_vehicle_wp.lane_id)
+ except:
+ pass
+
+ road_direction = current_wp.transform.get_forward_vector()
+ vehicle_direction = vehicle_transform.get_forward_vector()
+ dot_product = road_direction.x * vehicle_direction.x + road_direction.y * vehicle_direction.y
+
+ if dot_product < 0.0:
+ forward_wps = current_wp.next(10.0)
+ if forward_wps:
+ current_wp = forward_wps[0]
+ else:
+ current_wp = map.get_waypoint(
+ vehicle_transform.location + vehicle_direction * 5.0,
+ project_to_road=True
+ )
+
+ return current_wp
+
+def get_valid_spawn_points(map, count, base_location=None, radius=100.0):
+ """
+ 获取有效的出生点（增加容错性，避免索引越界）
+ """
+ # 1. 获取地图所有出生点
+ all_spawn_points = map.get_spawn_points()
+ if not all_spawn_points:
+ raise RuntimeError("地图中无任何出生点")
+
+ # 2. 初始化候选点列表
+ candidate_points = []
+
+ # 3. 如果有基准位置，先筛选附近的点；否则直接使用所有点
+ if base_location:
+ filtered_points = []
+ for sp in all_spawn_points:
+ dist = sp.location.distance(base_location)
+ if dist <= radius:
+ filtered_points.append((dist, sp))
+ # 按距离排序
+ filtered_points.sort(key=lambda x: x[0])
+ candidate_points = [sp for _, sp in filtered_points]
+
+ # 4. 如果候选点为空，直接使用所有出生点（容错）
+ if not candidate_points:
+ candidate_points = all_spawn_points
+ print(f"警告：基准位置{base_location}附近无出生点，使用全局出生点")
+
+ # 5. 筛选集中的出生点（放宽条件）
+ valid_points = []
+ # 确保基准点存在（核心修复：避免candidate_points[0]索引越界）
+ if not candidate_points:
+ candidate_points = all_spawn_points
+
+ base_sp = candidate_points[0]
+ valid_points.append(base_sp)
+
+ # 6. 筛选其他点，放宽距离限制
+ for sp in candidate_points[1:]:
+ try:
+ # 检查与已选点的距离（放宽到15米）
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+ except:
+ continue
+
+ # 7. 如果数量不够，进一步放宽条件（距离限制到20米）
+ if len(valid_points) < count:
+ for sp in candidate_points:
+ if sp not in valid_points:
+ try:
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT * 1.5 for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+ except:
+ continue
+
+ # 8. 如果还是不够，直接取前N个点（最终容错）
+ if len(valid_points) < count:
+ print(f"警告：无法找到{count}个集中的出生点，直接取前{count}个可用点")
+ for sp in candidate_points:
+ if sp not in valid_points:
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+
+ # 9. 最终检查：确保数量足够
+ if len(valid_points) < count:
+ # 直接取所有可用点，不足的话重复使用（极端情况）
+ while len(valid_points) < count:
+ valid_points.append(valid_points[0])
+ print(f"警告：出生点数量不足，重复使用已有点")
+
+ # 10. 统一出生点朝向
+ try:
+ forward_vec = valid_points[0].transform.get_forward_vector()
+ for sp in valid_points:
+ sp.rotation.yaw = math.degrees(math.atan2(forward_vec.y, forward_vec.x))
+ except:
+ pass
+
+ return valid_points[:count]
+
+def check_spawn_collision(world, spawn_point, radius=3.0):
+ # 检查周围车辆和行人
+ vehicles = world.get_actors().filter("vehicle.*")
+ for vehicle in vehicles:
+ if is_actor_alive(vehicle):
+ dist = vehicle.get_transform().location.distance(spawn_point.location)
+ if dist < radius:
+ return False
+
+ walkers = world.get_actors().filter("walker.*")
+ for walker in walkers:
+ if is_actor_alive(walker):
+ dist = walker.get_transform().location.distance(spawn_point.location)
+ if dist < radius:
+ return False
+
+ return True
+
+# ===================== 相机管理类（多车辆）=====================
+class VehicleCamera:
+ def __init__(self, world, vehicle, vehicle_id):
+ self.world = world
+ self.vehicle = vehicle
+ self.vehicle_id = vehicle_id
+ self.camera = None
+ self.image_surface = None
+
+ # 创建相机传感器
+ self._create_camera()
+
+ def _create_camera(self):
+ # 加载相机蓝图
+ camera_bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
+ camera_bp.set_attribute("image_size_x", str(640))
+ camera_bp.set_attribute("image_size_y", str(360))
+ camera_bp.set_attribute("fov", str(CAMERA_FOV))
+
+ # 生成相机（附加到车辆）
+ self.camera = self.world.spawn_actor(camera_bp, CAMERA_POS, attach_to=self.vehicle)
+
+ # 注册图像回调函数
+ self.camera.listen(self._on_image)
+
+ def _on_image(self, image):
+ # 将CARLA图像转换为Pygame Surface
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((image.height, image.width, 4))
+ array = array[:, :, :3]
+ array = array[:, :, ::-1]
+ array = np.swapaxes(array, 0, 1)
+
+ # 存储为Pygame Surface
+ self.image_surface = pygame.surfarray.make_surface(array)
+
+ def destroy(self):
+ if self.camera:
+ self.camera.stop()
+ self.camera.destroy()
+
+# ===================== 车辆控制类 ======================
+class VehicleAgent:
+ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_speed):
+ self.vehicle_id = vehicle_id
+ self.world = world
+ self.map = map
+ self.base_speed = base_speed
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+
+ # 生成车辆
+ self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
+ if self.vehicle_bp.has_attribute("color"):
+ color = random.choice(self.vehicle_bp.get_attribute("color").recommended_values)
+ self.vehicle_bp.set_attribute("color", color)
+
+ self.vehicle = self._spawn_vehicle_with_retry(spawn_point)
+ if not self.vehicle:
+ raise RuntimeError(f"车辆{vehicle_id}生成失败")
+
+ # 创建相机
+ self.camera = VehicleCamera(world, self.vehicle, vehicle_id)
+
+ # 初始化控制器
+ self.pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ self.speed_controller = SpeedController(PID_KP, PID_KI, PID_KD, base_speed)
+ self.traffic_light_manager = TrafficLightManager(vehicle_id)
+
+ self.is_alive = True
+ self.logger.info(f"生成成功，车型：{vehicle_model}，出生点：({spawn_point.location.x:.1f},{spawn_point.location.y:.1f})")
+
+ def _spawn_vehicle_with_retry(self, initial_spawn_point):
+ all_spawn_points = self.map.get_spawn_points()
+ if not all_spawn_points:
+ self.logger.error("地图中无有效出生点")
+ return None
+
+ candidate_points = [initial_spawn_point]
+ candidate_points += random.sample(all_spawn_points, min(10, len(all_spawn_points)))
+
+ for retry in range(SPAWN_RETRY_MAX):
+ spawn_point = candidate_points[retry % len(candidate_points)]
+ spawn_point.location.z += 0.3
+ spawn_point.rotation.yaw += random.randint(-5, 5)
+
+ if not check_spawn_collision(self.world, spawn_point):
+ self.logger.warning(f"第{retry+1}次重试：出生点有碰撞风险，跳过")
+ time.sleep(SPAWN_RETRY_DELAY)
+ continue
+
+ try:
+ return self.world.spawn_actor(self.vehicle_bp, spawn_point)
+ except Exception as e:
+ self.logger.warning(f"第{retry+1}次重试失败：{e}")
+ time.sleep(SPAWN_RETRY_DELAY)
+
+ self.logger.error(f"超过{SPAWN_RETRY_MAX}次重试，生成失败")
+ return None
+
+ def update(self):
+ if not self.is_alive or not is_actor_alive(self.vehicle):
+ self.is_alive = False
+ self.logger.error("车辆已销毁，停止更新")
+ return False
+
+ try:
+ # 获取车辆状态
+ vehicle_transform = self.vehicle.get_transform()
+ vehicle_vel = self.vehicle.get_velocity()
+ current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6
+
+ # 路径跟踪
+ current_wp = get_forward_waypoint(self.vehicle, self.map)
+ dir_change, curve_level = calculate_dir_change(current_wp)
+ lookahead_dist = self.pp_controller.get_adaptive_lookahead(dir_change)
+ target_wps = current_wp.next(lookahead_dist)
+ target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+
+ # 速度控制
+ curve_speed_factors = [1.0, 0.7, 0.4]
+ speed_factor = curve_speed_factors[min(curve_level, 2)]
+ base_target_speed = self.base_speed * speed_factor
+ base_target_speed = max(8.0, base_target_speed)
+
+ # 跟车控制
+ global vehicle_agents
+ if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
+ try:
+ lead_vehicle = vehicle_agents[self.vehicle_id - 2].vehicle
+ lead_vehicle_transform = lead_vehicle.get_transform()
+ dist_to_lead = vehicle_transform.location.distance(lead_vehicle_transform.location)
+ if dist_to_lead < 15.0:
+ base_target_speed = max(5.0, base_target_speed * 0.5)
+ except:
+ pass
+
+ # 交通灯处理
+ target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
+ self.vehicle, current_speed, base_target_speed
+ )
+
+ # 计算控制指令
+ steer = self.pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
+ throttle = self.speed_controller.calculate(target_speed, current_speed)
+ brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
+
+ if "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
+ throttle = 0.0
+ brake = 1.0
+
+ # 应用控制
+ control = carla.VehicleControl()
+ control.steer = steer
+ control.throttle = throttle
+ control.brake = brake
+ self.vehicle.apply_control(control)
+
+ # 日志输出
+ self.logger.info(
+ f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | "
+ f"弯道：{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态：{traffic_light_status}"
+ )
+
+ return True
+
+ except Exception as e:
+ self.logger.error(f"更新失败：{e}", exc_info=True)
+ return False
+
+ def destroy(self):
+ # 销毁相机
+ self.camera.destroy()
+ # 销毁车辆
+ if self.vehicle and is_actor_alive(self.vehicle):
+ self.vehicle.destroy()
+ self.logger.info("车辆资源已清理")
+
+# ===================== 控制器类 ======================
+class AdaptivePurePursuit:
+ def __init__(self, wheelbase):
+ self.wheelbase = wheelbase
+ self.last_steer = 0.0
+ self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT
+
+ def calculate_steer(self, vehicle_transform, target_point, dir_change):
+ forward_vec = vehicle_transform.get_forward_vector()
+ rear_axle_loc = carla.Location(
+ x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
+ y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
+ z=vehicle_transform.location.z
+ )
+
+ dx = target_point.x - rear_axle_loc.x
+ dy = target_point.y - rear_axle_loc.y
+ yaw = math.radians(vehicle_transform.rotation.yaw)
+
+ dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
+ dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
+
+ steer_gain = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
+ )
+ steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
+
+ if dx_vehicle < 0.1:
+ steer = self.last_steer
+ else:
+ steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle, dx_vehicle ** 2 + dy_vehicle ** 2)
+ steer = steer_rad / math.pi
+ steer *= steer_gain
+
+ if abs(steer) < STEER_DEADZONE:
+ steer = 0.0
+ steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
+ steer = np.clip(steer, -MAX_STEER, MAX_STEER)
+
+ self.last_steer = steer
+ return steer
+
+ def get_adaptive_lookahead(self, dir_change):
+ lookahead_dist = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
+ )
+ lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
+ self.last_lookahead = lookahead_dist
+ return lookahead_dist
+
+class SpeedController:
+ def __init__(self, kp, ki, kd, base_speed):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.base_speed = base_speed
+ self.last_error = 0.0
+ self.integral = 0.0
+
+ def calculate(self, target_speed, current_speed):
+ error = target_speed - current_speed
+ p = self.kp * error
+ self.integral += self.ki * error
+ self.integral = np.clip(self.integral, -1.0, 1.0)
+ i = self.integral
+ d = self.kd * (error - self.last_error)
+ self.last_error = error
+ return np.clip(p + i + d, 0.0, 1.0)
+
+class TrafficLightManager:
+ def __init__(self, vehicle_id):
+ self.vehicle_id = vehicle_id
+ self.tracked_light = None
+ self.is_stopped_at_red = False
+ self.red_light_stop_time = 0
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+
+ def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_transform):
+ vehicle_forward = vehicle_transform.get_forward_vector()
+ vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y])
+ vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward)
+
+ light_dir = light_transform.location - vehicle_transform.location
+ light_dir = np.array([light_dir.x, light_dir.y])
+ if np.linalg.norm(light_dir) < 0.1:
+ return 0.0
+ light_dir = light_dir / np.linalg.norm(light_dir)
+
+ angle = math.acos(np.clip(np.dot(vehicle_forward, light_dir), -1.0, 1.0))
+ angle = math.degrees(angle)
+ return angle
+
+ def get_lane_traffic_light(self, vehicle, world):
+ vehicle_transform = vehicle.get_transform()
+ vehicle_loc = vehicle_transform.location
+
+ if self.tracked_light and is_actor_alive(self.tracked_light):
+ dist = self.tracked_light.get_transform().location.distance(vehicle_loc)
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, self.tracked_light.get_transform())
+ if dist < TRAFFIC_LIGHT_DETECTION_RANGE and angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ return self.tracked_light
+
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ valid_lights = []
+
+ for light in traffic_lights:
+ if not is_actor_alive(light):
+ continue
+ dist = light.get_transform().location.distance(vehicle_loc)
+ if dist > TRAFFIC_LIGHT_DETECTION_RANGE:
+ continue
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, light.get_transform())
+ if angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ valid_lights.append((dist, light))
+
+ if valid_lights:
+ valid_lights.sort(key=lambda x: x[0])
+ self.tracked_light = valid_lights[0][1]
+ return self.tracked_light
+
+ self.tracked_light = None
+ return None
+
+ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
+ world = vehicle.get_world()
+ traffic_light = self.get_lane_traffic_light(vehicle, world)
+
+ if not traffic_light:
+ self.is_stopped_at_red = False
+ self.red_light_stop_time = 0
+ return base_target_speed, "No Light"
+
+ stop_line_loc = get_traffic_light_stop_line(traffic_light)
+ dist_to_stop_line = vehicle.get_transform().location.distance(stop_line_loc)
+
+ if traffic_light.get_state() == carla.TrafficLightState.Green:
+ if self.is_stopped_at_red:
+ recovery_speed = current_speed + (base_target_speed - current_speed) * GREEN_LIGHT_ACCEL_FACTOR
+ target_speed = max(STOP_SPEED_THRESHOLD, recovery_speed)
+ if abs(target_speed - base_target_speed) < 0.5:
+ self.is_stopped_at_red = False
+ self.logger.info(f"绿灯恢复行驶，目标速度：{target_speed:.1f}km/h")
+ return target_speed, "Green"
+ return base_target_speed, "Green"
+
+ elif traffic_light.get_state() == carla.TrafficLightState.Yellow:
+ self.is_stopped_at_red = False
+ yellow_speed = max(5.0, base_target_speed * 0.3)
+ self.logger.warning(f"黄灯减速，目标速度：{yellow_speed:.1f}km/h")
+ return yellow_speed, "Yellow"
+
+ elif traffic_light.get_state() == carla.TrafficLightState.Red:
+ if dist_to_stop_line > TRAFFIC_LIGHT_STOP_DISTANCE:
+ self.is_stopped_at_red = False
+ red_speed = max(2.0, current_speed * 0.1)
+ self.logger.warning(f"红灯减速，距离停止线：{dist_to_stop_line:.1f}m，目标速度：{red_speed:.1f}km/h")
+ return red_speed, "Red"
+ else:
+ if current_speed <= STOP_SPEED_THRESHOLD:
+ self.is_stopped_at_red = True
+ self.red_light_stop_time += 1
+ wait_seconds = self.red_light_stop_time // 30
+ self.logger.info(f"红灯停车等待：{wait_seconds}s")
+ return 0.0, "Red (Stopped)"
+ else:
+ self.logger.warning("红灯紧急制动")
+ return 0.0, "Red (Braking)"
+
+ return base_target_speed, "Unknown"
+
+# ===================== 交通灯控制线程 ======================
+def cycle_traffic_light_states(world, stop_event):
+ logger = logging.getLogger(__name__)
+ logger = logging.LoggerAdapter(logger, {"vehicle_id": "系统"})
+ while not stop_event.is_set():
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ if not traffic_lights:
+ time.sleep(1)
+ continue
+
+ # 红灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Red)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为红灯，持续{RED_LIGHT_DURATION}秒")
+ stop_event.wait(RED_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
+
+ # 绿灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Green)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为绿灯，持续{GREEN_LIGHT_DURATION}秒")
+ stop_event.wait(GREEN_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
+
+ # 黄灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Yellow)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为黄灯，持续{YELLOW_LIGHT_DURATION}秒")
+ stop_event.wait(YELLOW_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
+
+ logger.info("交通灯线程停止")
+
+# ===================== 主函数 ======================
+def main():
+ # ===================== ROS节点初始化（新增）======================
+ rospy.init_node(ROS_NODE_NAME, anonymous=True) # 初始化ROS节点
+ rospy.loginfo("===== Carla多车辆控制ROS节点启动 =====") # ROS日志（替代print）
+ status_pub = rospy.Publisher(VEHICLE_STATUS_TOPIC, String, queue_size=10) # 创建话题发布者
+ rate = rospy.Rate(30) # ROS循环频率（30Hz，和原代码的clock.tick(30)一致）
+ # ==============================================================
+
+ global current_view_vehicle_id, vehicle_agents
+ pygame.init()
+ screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
+ pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
+
+ client = None
+ world = None
+ map = None
+ tl_cycle_thread = None
+ tl_stop_event = threading.Event()
+ show_split_screen = True
+ show_top_view = False
+ top_view_camera = None
+ top_view_surface = None # 显式定义俯视相机表面变量
+
+ # 俯视相机更新函数
+ def _update_top_view(image):
+ nonlocal top_view_surface
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((image.height, image.width, 4))
+ array = array[:, :, :3]
+ array = array[:, :, ::-1]
+ array = np.swapaxes(array, 0, 1)
+ top_view_surface = pygame.surfarray.make_surface(array)
+
+ # 清理函数
+ def cleanup():
+ rospy.loginfo("\n开始清理资源...") # ROS日志
+ tl_stop_event.set()
+ if tl_cycle_thread and tl_cycle_thread.is_alive():
+ tl_cycle_thread.join(timeout=2)
+
+ if top_view_camera:
+ top_view_camera.stop()
+ top_view_camera.destroy()
+
+ for agent in vehicle_agents:
+ agent.destroy()
+
+ if world:
+ for actor in world.get_actors():
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
+ if is_actor_alive(actor):
+ actor.destroy()
+
+ pygame.quit()
+ rospy.loginfo("资源清理完成") # ROS日志
+
+ # 注册退出回调
+ import atexit
+ import signal
+ atexit.register(cleanup)
+ signal.signal(signal.SIGINT, lambda sig, frame: sys.exit(0))
+
+ try:
+ # 连接CARLA
+ client = carla.Client(CARLA_HOST, CARLA_PORT)
+ client.set_timeout(CARLA_TIMEOUT)
+ try:
+ world = client.load_world("Town04")
+ rospy.loginfo("成功加载Town04地图")
+ except Exception as e:
+ world = client.get_world()
+ rospy.logwarn(f"警告：Town04地图加载失败（{e}），使用当前地图")
+ map = world.get_map()
+
+ # 清理残留演员
+ rospy.loginfo("清理残留演员...")
+ for actor in world.get_actors():
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
+ if is_actor_alive(actor):
+ actor.destroy()
+ time.sleep(3.0)
+ rospy.loginfo("清理完成")
+
+ # 自动获取地图的第一个出生点作为基准（避免手动坐标无效）
+ base_location = None
+ all_spawn_points = map.get_spawn_points()
+ if all_spawn_points:
+ base_location = all_spawn_points[0].location
+ rospy.loginfo(f"使用地图第一个出生点作为基准：({base_location.x:.1f}, {base_location.y:.1f})")
+ else:
+ base_location = carla.Location(x=220.0, y=150.0, z=0.5)
+
+ # 获取有效的出生点
+ rospy.loginfo(f"获取{VEHICLE_COUNT}个有效出生点...")
+ valid_spawn_points = get_valid_spawn_points(map, VEHICLE_COUNT, base_location)
+ for i, sp in enumerate(valid_spawn_points):
+ rospy.loginfo(f" 出生点{i+1}：({sp.location.x:.1f},{sp.location.y:.1f})")
+
+ # 生成车辆
+ rospy.loginfo(f"\n分步生成车辆（间隔{SPAWN_INTERVAL}秒）...")
+ for i in range(VEHICLE_COUNT):
+ vehicle_model = VEHICLE_MODELS[i % len(VEHICLE_MODELS)]
+ base_speed = BASE_SPEEDS[i % len(BASE_SPEEDS)]
+ spawn_point = valid_spawn_points[i]
+
+ try:
+ rospy.loginfo(f"\n生成车辆{i+1}（车型：{vehicle_model}）...")
+ agent = VehicleAgent(world, map, i+1, spawn_point, vehicle_model, base_speed)
+ vehicle_agents.append(agent)
+ rospy.loginfo(f"车辆{i+1}生成成功！")
+ except Exception as e:
+ rospy.logerr(f"车辆{i+1}生成失败：{e}")
+
+ time.sleep(SPAWN_INTERVAL)
+
+ if len(vehicle_agents) == 0:
+ raise RuntimeError("无车辆生成成功，仿真终止")
+
+ rospy.loginfo(f"\n共生成{len(vehicle_agents)}辆车辆！")
+
+ # 创建全局俯视相机
+ try:
+ top_view_bp = world.get_blueprint_library().find("sensor.camera.rgb")
+ top_view_bp.set_attribute("image_size_x", str(WINDOW_WIDTH))
+ top_view_bp.set_attribute("image_size_y", str(WINDOW_HEIGHT))
+ top_view_bp.set_attribute("fov", str(90))
+ top_view_transform = carla.Transform(
+ vehicle_agents[0].vehicle.get_transform().location + carla.Location(z=50),
+ carla.Rotation(pitch=-90)
+ )
+ top_view_camera = world.spawn_actor(top_view_bp, top_view_transform)
+ top_view_camera.listen(_update_top_view)
+ except:
+ rospy.logwarn("警告：无法创建俯视相机")
+
+ # 启动交通灯线程
+ tl_cycle_thread = threading.Thread(target=cycle_traffic_light_states, args=(world, tl_stop_event), daemon=True)
+ tl_cycle_thread.start()
+ rospy.loginfo("交通灯线程启动")
+
+ # 主循环
+ clock = pygame.time.Clock()
+ running = True
+
+ # 核心修改：加入ROS关闭信号检测
+ while not rospy.is_shutdown() and running:
+ # 事件处理
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ running = False
+ elif event.type == pygame.KEYDOWN:
+ if event.key == pygame.K_1 and len(vehicle_agents) >= 1:
+ current_view_vehicle_id = 1
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_2 and len(vehicle_agents) >= 2:
+ current_view_vehicle_id = 2
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_3 and len(vehicle_agents) >= 3:
+ current_view_vehicle_id = 3
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_s:
+ show_split_screen = True
+ show_top_view = False
+ elif event.key == pygame.K_v:
+ show_top_view = True
+ show_split_screen = False
+ elif event.key == pygame.K_ESCAPE:
+ running = False
+
+ # 清空屏幕
+ screen.fill((0, 0, 0))
+
+ if show_top_view:
+ if top_view_surface:
+ screen.blit(top_view_surface, (0, 0))
+ elif show_split_screen:
+ if len(vehicle_agents) == 1:
+ agent = vehicle_agents[0]
+ if agent.camera.image_surface:
+ surface = pygame.transform.scale(agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surface, (0, 0))
+ elif len(vehicle_agents) == 2:
+ agent1 = vehicle_agents[0]
+ agent2 = vehicle_agents[1]
+
+ if agent1.camera.image_surface:
+ surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ screen.blit(surface1, (0, 0))
+
+ if agent2.camera.image_surface:
+ surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ screen.blit(surface2, (WINDOW_WIDTH//2, 0))
+ elif len(vehicle_agents) >= 3:
+ agent1 = vehicle_agents[0]
+ agent2 = vehicle_agents[1]
+ agent3 = vehicle_agents[2]
+
+ if agent1.camera.image_surface:
+ surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surface1, (0, 0))
+
+ if agent2.camera.image_surface:
+ surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surface2, (WINDOW_WIDTH//2, 0))
+
+ if agent3.camera.image_surface:
+ surface3 = pygame.transform.scale(agent3.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
+ screen.blit(surface3, (0, WINDOW_HEIGHT//2))
+ else:
+ target_agent = None
+ for agent in vehicle_agents:
+ if agent.vehicle_id == current_view_vehicle_id:
+ target_agent = agent
+ break
+
+ if target_agent and target_agent.camera.image_surface:
+ surface = pygame.transform.scale(target_agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surface, (0, 0))
+
+ # 更新车辆状态
+ with ThreadPoolExecutor(max_workers=VEHICLE_COUNT) as executor:
+ futures = [executor.submit(agent.update) for agent in vehicle_agents]
+ for future in as_completed(futures):
+ try:
+ future.result()
+ except Exception as e:
+ rospy.logerr(f"车辆更新异常：{e}")
+
+ # 刷新屏幕
+ pygame.display.flip()
+ clock.tick(30)
+
+ # ===================== ROS话题发布（核心新增）======================
+ status_msg = String()
+ status_msg.data = f"当前车辆数：{len(vehicle_agents)}，当前视角车辆：{current_view_vehicle_id}"
+ status_pub.publish(status_msg)
+ rate.sleep() # ROS频率控制
+ # ==============================================================
+
+ except Exception as e:
+ rospy.logerr(f"仿真异常：{e}") # ROS错误日志
+ traceback.print_exc()
+ finally:
+ cleanup()
+ rospy.loginfo("===== Carla多车辆控制ROS节点停止 =====")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/autonomous_driving_car/ros/carla_vehicle_control.py b/src/autonomous_driving_car/ros/carla_vehicle_control.py
new file mode 100644
index 0000000000..9a002e7bea
--- /dev/null
+++ b/src/autonomous_driving_car/ros/carla_vehicle_control.py
@@ -0,0 +1,908 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+CARLA 多车辆协同控制版：修复出生点索引越界问题
+"""
+
+import sys
+import os
+import carla
+import numpy as np
+import math
+import pygame
+import traceback
+import time
+import threading
+from concurrent.futures import ThreadPoolExecutor, as_completed
+import logging
+import random
+
+# ===================== 全局配置 =======================
+# CARLA连接
+CARLA_HOST = "192.168.137.112" # 替换为你的主机IPv4地址
+CARLA_PORT = 2000
+CARLA_TIMEOUT = 30.0 # 超时时间从20秒增加到30秒，提高容错性
+
+# 多车辆配置
+VEHICLE_COUNT = 3
+VEHICLE_MODELS = [
+ "vehicle.tesla.model3",
+ "vehicle.bmw.grandtourer",
+ "vehicle.audi.a2"
+]
+SPAWN_INTERVAL = 1.0
+SPAWN_RETRY_MAX = 8
+SPAWN_RETRY_DELAY = 0.5
+SPAWN_DISTANCE_LIMIT = 15.0 # 放宽距离限制到15米
+
+# 车辆控制参数
+VEHICLE_WHEELBASE = 2.9
+VEHICLE_REAR_AXLE_OFFSET = 1.45
+LOOKAHEAD_DIST_STRAIGHT = 7.0
+LOOKAHEAD_DIST_CURVE = 4.0
+STEER_GAIN_STRAIGHT = 0.7
+STEER_GAIN_CURVE = 1.0
+STEER_DEADZONE = 0.05
+STEER_LOWPASS_ALPHA = 0.6
+MAX_STEER = 1.0
+DIR_CHANGE_GENTLE = 0.03
+DIR_CHANGE_SHARP = 0.08
+BASE_SPEEDS = [25.0, 22.0, 20.0]
+PID_KP = 0.2
+PID_KI = 0.01
+PID_KD = 0.02
+
+# 交通规则配置
+TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
+TRAFFIC_LIGHT_DETECTION_RANGE = 50.0
+TRAFFIC_LIGHT_ANGLE_THRESHOLD = 60.0
+STOP_SPEED_THRESHOLD = 0.2
+GREEN_LIGHT_ACCEL_FACTOR = 0.25
+STOP_LINE_SIM_DISTANCE = 5.0
+RED_LIGHT_DURATION = 3.0
+GREEN_LIGHT_DURATION = 5.0
+YELLOW_LIGHT_DURATION = 2.0
+
+# 相机配置
+WINDOW_WIDTH = 1280
+WINDOW_HEIGHT = 720
+CAMERA_FOV = 120
+CAMERA_POS = carla.Transform(carla.Location(x=-6.0, z=2.5), carla.Rotation(pitch=-5))
+
+# 全局变量
+current_view_vehicle_id = 1
+vehicle_agents = []
+
+# 日志配置
+logging.basicConfig(
+ level=logging.INFO,
+ format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
+ handlers=[logging.FileHandler("multi_vehicle_simulation.log"), logging.StreamHandler()]
+)
+
+# ===================== 核心工具函数 ======================
+def is_actor_alive(actor):
+ try:
+ return actor.is_alive()
+ except TypeError:
+ return actor.is_alive
+
+def get_traffic_light_stop_line(traffic_light):
+ try:
+ return traffic_light.get_stop_line_location()
+ except AttributeError:
+ tl_transform = traffic_light.get_transform()
+ forward_vec = tl_transform.get_forward_vector()
+ stop_line_loc = tl_transform.location - forward_vec * STOP_LINE_SIM_DISTANCE
+ stop_line_loc.z = tl_transform.location.z
+ return stop_line_loc
+
+def calculate_dir_change(current_wp):
+ waypoints = [current_wp]
+ for i in range(5):
+ next_wps = waypoints[-1].next(1.0)
+ if next_wps:
+ waypoints.append(next_wps[0])
+ else:
+ break
+
+ if len(waypoints) < 4:
+ return 0.0, 0
+
+ dirs = []
+ for i in range(1, len(waypoints)):
+ wp_prev = waypoints[i-1]
+ wp_curr = waypoints[i]
+ dir_rad = math.atan2(
+ wp_curr.transform.location.y - wp_prev.transform.location.y,
+ wp_curr.transform.location.x - wp_prev.transform.location.x
+ )
+ dirs.append(dir_rad)
+
+ dir_change = 0.0
+ for i in range(1, len(dirs)):
+ dir_change += abs(dirs[i] - dirs[i-1]) * 2
+
+ if dir_change < DIR_CHANGE_GENTLE:
+ curve_level = 0
+ elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 1
+ else:
+ curve_level = 2
+
+ return dir_change, curve_level
+
+def get_forward_waypoint(vehicle, map):
+ vehicle_transform = vehicle.get_transform()
+ current_wp = map.get_waypoint(
+ vehicle_transform.location,
+ project_to_road=True,
+ lane_type=carla.LaneType.Driving
+ )
+
+ # 所有车辆使用同一车道的路点
+ global vehicle_agents
+ if len(vehicle_agents) > 0:
+ try:
+ lead_vehicle_wp = map.get_waypoint(vehicle_agents[0].vehicle.get_transform().location, project_to_road=True)
+ current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True, lane_id=lead_vehicle_wp.lane_id)
+ except:
+ pass
+
+ road_direction = current_wp.transform.get_forward_vector()
+ vehicle_direction = vehicle_transform.get_forward_vector()
+ dot_product = road_direction.x * vehicle_direction.x + road_direction.y * vehicle_direction.y
+
+ if dot_product < 0.0:
+ forward_wps = current_wp.next(10.0)
+ if forward_wps:
+ current_wp = forward_wps[0]
+ else:
+ current_wp = map.get_waypoint(
+ vehicle_transform.location + vehicle_direction * 5.0,
+ project_to_road=True
+ )
+
+ return current_wp
+
+def get_valid_spawn_points(map, count, base_location=None, radius=100.0):
+ """
+ 获取有效的出生点（增加容错性，避免索引越界）
+ """
+ # 1. 获取地图所有出生点
+ all_spawn_points = map.get_spawn_points()
+ if not all_spawn_points:
+ raise RuntimeError("地图中无任何出生点")
+
+ # 2. 初始化候选点列表
+ candidate_points = []
+
+ # 3. 如果有基准位置，先筛选附近的点；否则直接使用所有点
+ if base_location:
+ filtered_points = []
+ for sp in all_spawn_points:
+ dist = sp.location.distance(base_location)
+ if dist <= radius:
+ filtered_points.append((dist, sp))
+ # 按距离排序
+ filtered_points.sort(key=lambda x: x[0])
+ candidate_points = [sp for _, sp in filtered_points]
+
+ # 4. 如果候选点为空，直接使用所有出生点（容错）
+ if not candidate_points:
+ candidate_points = all_spawn_points
+ print(f"警告：基准位置{base_location}附近无出生点，使用全局出生点")
+
+ # 5. 筛选集中的出生点（放宽条件）
+ valid_points = []
+ # 确保基准点存在（核心修复：避免candidate_points[0]索引越界）
+ if not candidate_points:
+ candidate_points = all_spawn_points
+
+ base_sp = candidate_points[0]
+ valid_points.append(base_sp)
+
+ # 6. 筛选其他点，放宽距离限制
+ for sp in candidate_points[1:]:
+ try:
+ # 检查与已选点的距离（放宽到15米）
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+ except:
+ continue
+
+ # 7. 如果数量不够，进一步放宽条件（距离限制到20米）
+ if len(valid_points) < count:
+ for sp in candidate_points:
+ if sp not in valid_points:
+ try:
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT * 1.5 for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+ except:
+ continue
+
+ # 8. 如果还是不够，直接取前N个点（最终容错）
+ if len(valid_points) < count:
+ print(f"警告：无法找到{count}个集中的出生点，直接取前{count}个可用点")
+ for sp in candidate_points:
+ if sp not in valid_points:
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ if len(valid_points) >= count:
+ break
+
+ # 9. 最终检查：确保数量足够
+ if len(valid_points) < count:
+ # 直接取所有可用点，不足的话重复使用（极端情况）
+ while len(valid_points) < count:
+ valid_points.append(valid_points[0])
+ print(f"警告：出生点数量不足，重复使用已有点")
+
+ # 10. 统一出生点朝向
+ try:
+ forward_vec = valid_points[0].transform.get_forward_vector()
+ for sp in valid_points:
+ sp.rotation.yaw = math.degrees(math.atan2(forward_vec.y, forward_vec.x))
+ except:
+ pass
+
+ return valid_points[:count]
+
+def check_spawn_collision(world, spawn_point, radius=3.0):
+ # 检查周围车辆和行人
+ vehicles = world.get_actors().filter("vehicle.*")
+ for vehicle in vehicles:
+ if is_actor_alive(vehicle):
+ dist = vehicle.get_transform().location.distance(spawn_point.location)
+ if dist < radius:
+ return False
+
+ walkers = world.get_actors().filter("walker.*")
+ for walker in walkers:
+ if is_actor_alive(walker):
+ dist = walker.get_transform().location.distance(spawn_point.location)
+ if dist < radius:
+ return False
+
+ return True
+
+# ===================== 相机管理类（多车辆）=====================
+class VehicleCamera:
+ def __init__(self, world, vehicle, vehicle_id):
+ self.world = world
+ self.vehicle = vehicle
+ self.vehicle_id = vehicle_id
+ self.camera = None
+ self.image_surface = None
+
+ # 创建相机传感器
+ self._create_camera()
+
+ def _create_camera(self):
+ # 加载相机蓝图
+ camera_bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
+ camera_bp.set_attribute("image_size_x", str(640))
+ camera_bp.set_attribute("image_size_y", str(360))
+ camera_bp.set_attribute("fov", str(CAMERA_FOV))
+
+ # 生成相机（附加到车辆）
+ self.camera = self.world.spawn_actor(camera_bp, CAMERA_POS, attach_to=self.vehicle)
+
+ # 注册图像回调函数
+ self.camera.listen(self._on_image)
+
+ def _on_image(self, image):
+ # 将CARLA图像转换为Pygame Surface
+ array = np.frombuffer(image.raw_data, dtype=np.uint8)
+ array = array.reshape((image.height, image.width, 4))
+ array = array[:, :, :3]
+ array = array[:, :, ::-1]
+ array = np.swapaxes(array, 0, 1)
+
+ # 存储为Pygame Surface
+ self.image_surface = pygame.surfarray.make_surface(array)
+
+ def destroy(self):
+ if self.camera:
+ self.camera.stop()
+ self.camera.destroy()
+
+# ===================== 车辆控制类 ======================
+class VehicleAgent:
+ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_speed):
+ self.vehicle_id = vehicle_id
+ self.world = world
+ self.map = map
+ self.base_speed = base_speed
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+
+ # 生成车辆
+ self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
+ if self.vehicle_bp.has_attribute("color"):
+ color = random.choice(self.vehicle_bp.get_attribute("color").recommended_values)
+ self.vehicle_bp.set_attribute("color", color)
+
+ self.vehicle = self._spawn_vehicle_with_retry(spawn_point)
+ if not self.vehicle:
+ raise RuntimeError(f"车辆{vehicle_id}生成失败")
+
+ # 创建相机
+ self.camera = VehicleCamera(world, self.vehicle, vehicle_id)
+
+ # 初始化控制器
+ self.pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ self.speed_controller = SpeedController(PID_KP, PID_KI, PID_KD, base_speed)
+ self.traffic_light_manager = TrafficLightManager(vehicle_id)
+
+ self.is_alive = True
+ self.logger.info(f"生成成功，车型：{vehicle_model}，出生点：({spawn_point.location.x:.1f},{spawn_point.location.y:.1f})")
+
+ def _spawn_vehicle_with_retry(self, initial_spawn_point):
+ all_spawn_points = self.map.get_spawn_points()
+ if not all_spawn_points:
+ self.logger.error("地图中无有效出生点")
+ return None
+
+ candidate_points = [initial_spawn_point]
+ candidate_points += random.sample(all_spawn_points, min(10, len(all_spawn_points)))
+
+ for retry in range(SPAWN_RETRY_MAX):
+ spawn_point = candidate_points[retry % len(candidate_points)]
+ spawn_point.location.z += 0.3
+ spawn_point.rotation.yaw += random.randint(-5, 5)
+
+ if not check_spawn_collision(self.world, spawn_point):
+ self.logger.warning(f"第{retry+1}次重试：出生点有碰撞风险，跳过")
+ time.sleep(SPAWN_RETRY_DELAY)
+ continue
+
+ try:
+ return self.world.spawn_actor(self.vehicle_bp, spawn_point)
+ except Exception as e:
+ self.logger.warning(f"第{retry+1}次重试失败：{e}")
+ time.sleep(SPAWN_RETRY_DELAY)
+
+ self.logger.error(f"超过{SPAWN_RETRY_MAX}次重试，生成失败")
+ return None
+
+ def update(self):
+ if not self.is_alive or not is_actor_alive(self.vehicle):
+ self.is_alive = False
+ self.logger.error("车辆已销毁，停止更新")
+ return False
+
+ try:
+ # 获取车辆状态
+ vehicle_transform = self.vehicle.get_transform()
+ vehicle_vel = self.vehicle.get_velocity()
+ current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6
+
+ # 路径跟踪
+ current_wp = get_forward_waypoint(self.vehicle, self.map)
+ dir_change, curve_level = calculate_dir_change(current_wp)
+ lookahead_dist = self.pp_controller.get_adaptive_lookahead(dir_change)
+ target_wps = current_wp.next(lookahead_dist)
+ target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
+
+ # 速度控制
+ curve_speed_factors = [1.0, 0.7, 0.4]
+ speed_factor = curve_speed_factors[min(curve_level, 2)]
+ base_target_speed = self.base_speed * speed_factor
+ base_target_speed = max(8.0, base_target_speed)
+
+ # 跟车控制
+ global vehicle_agents
+ if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
+ try:
+ lead_vehicle = vehicle_agents[self.vehicle_id - 2].vehicle
+ lead_vehicle_transform = lead_vehicle.get_transform()
+ dist_to_lead = vehicle_transform.location.distance(lead_vehicle_transform.location)
+ if dist_to_lead < 15.0:
+ base_target_speed = max(5.0, base_target_speed * 0.5)
+ except:
+ pass
+
+ # 交通灯处理
+ target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
+ self.vehicle, current_speed, base_target_speed
+ )
+
+ # 计算控制指令
+ steer = self.pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
+ throttle = self.speed_controller.calculate(target_speed, current_speed)
+ brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
+
+ if "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
+ throttle = 0.0
+ brake = 1.0
+
+ # 应用控制
+ control = carla.VehicleControl()
+ control.steer = steer
+ control.throttle = throttle
+ control.brake = brake
+ self.vehicle.apply_control(control)
+
+ # 日志输出
+ self.logger.info(
+ f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | "
+ f"弯道：{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态：{traffic_light_status}"
+ )
+
+ return True
+
+ except Exception as e:
+ self.logger.error(f"更新失败：{e}", exc_info=True)
+ return False
+
+ def destroy(self):
+ # 销毁相机
+ self.camera.destroy()
+ # 销毁车辆
+ if self.vehicle and is_actor_alive(self.vehicle):
+ self.vehicle.destroy()
+ self.logger.info("车辆资源已清理")
+
+# ===================== 控制器类 ======================
+class AdaptivePurePursuit:
+ def __init__(self, wheelbase):
+ self.wheelbase = wheelbase
+ self.last_steer = 0.0
+ self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT
+
+ def calculate_steer(self, vehicle_transform, target_point, dir_change):
+ forward_vec = vehicle_transform.get_forward_vector()
+ rear_axle_loc = carla.Location(
+ x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
+ y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
+ z=vehicle_transform.location.z
+ )
+
+ dx = target_point.x - rear_axle_loc.x
+ dy = target_point.y - rear_axle_loc.y
+ yaw = math.radians(vehicle_transform.rotation.yaw)
+
+ dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
+ dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
+
+ steer_gain = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
+ )
+ steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
+
+ if dx_vehicle < 0.1:
+ steer = self.last_steer
+ else:
+ steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle, dx_vehicle ** 2 + dy_vehicle ** 2)
+ steer = steer_rad / math.pi
+ steer *= steer_gain
+
+ if abs(steer) < STEER_DEADZONE:
+ steer = 0.0
+ steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
+ steer = np.clip(steer, -MAX_STEER, MAX_STEER)
+
+ self.last_steer = steer
+ return steer
+
+ def get_adaptive_lookahead(self, dir_change):
+ lookahead_dist = np.interp(
+ dir_change,
+ [0, DIR_CHANGE_SHARP],
+ [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
+ )
+ lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
+ self.last_lookahead = lookahead_dist
+ return lookahead_dist
+
+class SpeedController:
+ def __init__(self, kp, ki, kd, base_speed):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.base_speed = base_speed
+ self.last_error = 0.0
+ self.integral = 0.0
+
+ def calculate(self, target_speed, current_speed):
+ error = target_speed - current_speed
+ p = self.kp * error
+ self.integral += self.ki * error
+ self.integral = np.clip(self.integral, -1.0, 1.0)
+ i = self.integral
+ d = self.kd * (error - self.last_error)
+ self.last_error = error
+ return np.clip(p + i + d, 0.0, 1.0)
+
+class TrafficLightManager:
+ def __init__(self, vehicle_id):
+ self.vehicle_id = vehicle_id
+ self.tracked_light = None
+ self.is_stopped_at_red = False
+ self.red_light_stop_time = 0
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+
+ def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_transform):
+ vehicle_forward = vehicle_transform.get_forward_vector()
+ vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y])
+ vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward)
+
+ light_dir = light_transform.location - vehicle_transform.location
+ light_dir = np.array([light_dir.x, light_dir.y])
+ if np.linalg.norm(light_dir) < 0.1:
+ return 0.0
+ light_dir = light_dir / np.linalg.norm(light_dir)
+
+ angle = math.acos(np.clip(np.dot(vehicle_forward, light_dir), -1.0, 1.0))
+ angle = math.degrees(angle)
+ return angle
+
+ def get_lane_traffic_light(self, vehicle, world):
+ vehicle_transform = vehicle.get_transform()
+ vehicle_loc = vehicle_transform.location
+
+ if self.tracked_light and is_actor_alive(self.tracked_light):
+ dist = self.tracked_light.get_transform().location.distance(vehicle_loc)
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, self.tracked_light.get_transform())
+ if dist < TRAFFIC_LIGHT_DETECTION_RANGE and angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ return self.tracked_light
+
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ valid_lights = []
+
+ for light in traffic_lights:
+ if not is_actor_alive(light):
+ continue
+ dist = light.get_transform().location.distance(vehicle_loc)
+ if dist > TRAFFIC_LIGHT_DETECTION_RANGE:
+ continue
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, light.get_transform())
+ if angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ valid_lights.append((dist, light))
+
+ if valid_lights:
+ valid_lights.sort(key=lambda x: x[0])
+ self.tracked_light = valid_lights[0][1]
+ return self.tracked_light
+
+ self.tracked_light = None
+ return None
+
+ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
+ world = vehicle.get_world()
+ traffic_light = self.get_lane_traffic_light(vehicle, world)
+
+ if not traffic_light:
+ self.is_stopped_at_red = False
+ self.red_light_stop_time = 0
+ return base_target_speed, "No Light"
+
+ stop_line_loc = get_traffic_light_stop_line(traffic_light)
+ dist_to_stop_line = vehicle.get_transform().location.distance(stop_line_loc)
+
+ if traffic_light.get_state() == carla.TrafficLightState.Green:
+ if self.is_stopped_at_red:
+ recovery_speed = current_speed + (base_target_speed - current_speed) * GREEN_LIGHT_ACCEL_FACTOR
+ target_speed = max(STOP_SPEED_THRESHOLD, recovery_speed)
+ if abs(target_speed - base_target_speed) < 0.5:
+ self.is_stopped_at_red = False
+ self.logger.info(f"绿灯恢复行驶，目标速度：{target_speed:.1f}km/h")
+ return target_speed, "Green"
+ return base_target_speed, "Green"
+
+ elif traffic_light.get_state() == carla.TrafficLightState.Yellow:
+ self.is_stopped_at_red = False
+ yellow_speed = max(5.0, base_target_speed * 0.3)
+ self.logger.warning(f"黄灯减速，目标速度：{yellow_speed:.1f}km/h")
+ return yellow_speed, "Yellow"
+
+ elif traffic_light.get_state() == carla.TrafficLightState.Red:
+ if dist_to_stop_line > TRAFFIC_LIGHT_STOP_DISTANCE:
+ self.is_stopped_at_red = False
+ red_speed = max(2.0, current_speed * 0.1)
+ self.logger.warning(f"红灯减速，距离停止线：{dist_to_stop_line:.1f}m，目标速度：{red_speed:.1f}km/h")
+ return red_speed, "Red"
+ else:
+ if current_speed <= STOP_SPEED_THRESHOLD:
+ self.is_stopped_at_red = True
+ self.red_light_stop_time += 1
+ wait_seconds = self.red_light_stop_time // 30
+ self.logger.info(f"红灯停车等待：{wait_seconds}s")
+ return 0.0, "Red (Stopped)"
+ else:
+ self.logger.warning("红灯紧急制动")
+ return 0.0, "Red (Braking)"
+
+ return base_target_speed, "Unknown"
+
+# ===================== 交通灯控制线程 ======================
+def cycle_traffic_light_states(world, stop_event):
+ logger = logging.getLogger(__name__)
+ logger = logging.LoggerAdapter(logger, {"vehicle_id": "系统"})
+ while not stop_event.is_set():
+ traffic_lights = world.get_actors().filter("traffic.traffic_light")
+ if not traffic_lights:
+ time.sleep(1)
+ continue
+
+ # 红灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Red)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为红灯，持续{RED_LIGHT_DURATION}秒")
+ stop_event.wait(RED_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
+
+ # 绿灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Green)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为绿灯，持续{GREEN_LIGHT_DURATION}秒")
+ stop_event.wait(GREEN_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
+
+ # 黄灯
+ for tl in traffic_lights:
+ if is_actor_alive(tl):
+ try:
+ tl.set_state(carla.TrafficLightState.Yellow)
+ except:
+ pass
+ logger.info(f"所有交通灯切换为黄灯，持续{YELLOW_LIGHT_DURATION}秒")
+ stop_event.wait(YELLOW_LIGHT_DURATION)
+ if stop_event.is_set():
+ break
+
+ logger.info("交通灯线程停止")
+
+# ===================== 主函数 ======================
+def main():
+ global current_view_vehicle_id, vehicle_agents
+ pygame.init()
+ screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
+ pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
+
+ client = None
+ world = None
+ map = None
+ tl_cycle_thread = None
+ tl_stop_event = threading.Event()
+ show_split_screen = True
+ show_top_view = False
+ top_view_camera = None
+
+ # 清理函数
+ def cleanup():
+ print("\n开始清理资源...")
+ tl_stop_event.set()
+ if tl_cycle_thread and tl_cycle_thread.is_alive():
+ tl_cycle_thread.join(timeout=2)
+
+ if top_view_camera:
+ top_view_camera.stop()
+ top_view_camera.destroy()
+
+ for agent in vehicle_agents:
+ agent.destroy()
+
+ if world:
+ for actor in world.get_actors():
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
+ if is_actor_alive(actor):
+ actor.destroy()
+
+ pygame.quit()
+ print("资源清理完成")
+
+ # 注册退出回调
+ import atexit
+ import signal
+ atexit.register(cleanup)
+ signal.signal(signal.SIGINT, lambda sig, frame: sys.exit(0))
+
+ try:
+ # 连接CARLA
+ client = carla.Client(CARLA_HOST, CARLA_PORT)
+ client.set_timeout(CARLA_TIMEOUT)
+ try:
+ world = client.load_world("Town04")
+ print("成功加载Town04地图")
+ except Exception as e:
+ world = client.get_world()
+ print(f"警告：Town04地图加载失败（{e}），使用当前地图")
+ map = world.get_map()
+
+ # 清理残留演员
+ print("清理残留演员...")
+ for actor in world.get_actors():
+ if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
+ if is_actor_alive(actor):
+ actor.destroy()
+ time.sleep(3.0)
+ print("清理完成")
+
+ # 自动获取地图的第一个出生点作为基准（避免手动坐标无效）
+ base_location = None
+ all_spawn_points = map.get_spawn_points()
+ if all_spawn_points:
+ base_location = all_spawn_points[0].location
+ print(f"使用地图第一个出生点作为基准：({base_location.x:.1f}, {base_location.y:.1f})")
+ else:
+ base_location = carla.Location(x=220.0, y=150.0, z=0.5)
+
+ # 获取有效的出生点
+ print(f"获取{VEHICLE_COUNT}个有效出生点...")
+ valid_spawn_points = get_valid_spawn_points(map, VEHICLE_COUNT, base_location)
+ for i, sp in enumerate(valid_spawn_points):
+ print(f" 出生点{i+1}：({sp.location.x:.1f},{sp.location.y:.1f})")
+
+ # 生成车辆
+ print(f"\n分步生成车辆（间隔{SPAWN_INTERVAL}秒）...")
+ for i in range(VEHICLE_COUNT):
+ vehicle_model = VEHICLE_MODELS[i % len(VEHICLE_MODELS)]
+ base_speed = BASE_SPEEDS[i % len(BASE_SPEEDS)]
+ spawn_point = valid_spawn_points[i]
+
+ try:
+ print(f"\n生成车辆{i+1}（车型：{vehicle_model}）...")
+ agent = VehicleAgent(world, map, i+1, spawn_point, vehicle_model, base_speed)
+ vehicle_agents.append(agent)
+ print(f"车辆{i+1}生成成功！")
+ except Exception as e:
+ print(f"车辆{i+1}生成失败：{e}")
+
+ time.sleep(SPAWN_INTERVAL)
+
+ if len(vehicle_agents) == 0:
+ raise RuntimeError("无车辆生成成功，仿真终止")
+
+ print(f"\n共生成{len(vehicle_agents)}辆车辆！")
+
+ # 创建全局俯视相机
+ try:
+ top_view_bp = world.get_blueprint_library().find("sensor.camera.rgb")
+ top_view_bp.set_attribute("image_size_x", str(WINDOW_WIDTH))
+ top_view_bp.set_attribute("image_size_y", str(WINDOW_HEIGHT))
+ top_view_bp.set_attribute("fov", str(90))
+ top_view_transform = carla.Transform(
+ vehicle_agents[0].vehicle.get_transform().location + carla.Location(z=50),
+ carla.Rotation(pitch=-90)
+ )
+ top_view_camera = world.spawn_actor(top_view_bp, top_view_transform)
+ top_view_surface = None
+ top_view_camera.listen(lambda image: globals().update({
+ "top_view_surface": pygame.surfarray.make_surface(
+ np.swapaxes(np.array(image.raw_data).reshape((image.height, image.width, 4))[:, :, :3][:, :, ::-1], 0, 1)
+ )
+ }))
+ except:
+ print("警告：无法创建俯视相机")
+
+ # 启动交通灯线程
+ tl_cycle_thread = threading.Thread(target=cycle_traffic_light_states, args=(world, tl_stop_event), daemon=True)
+ tl_cycle_thread.start()
+ print("交通灯线程启动")
+
+ # 主循环
+ clock = pygame.time.Clock()
+ running = True
+
+ while running:
+ # 事件处理
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ running = False
+ elif event.type == pygame.KEYDOWN:
+ if event.key == pygame.K_1 and len(vehicle_agents) >= 1:
+ current_view_vehicle_id = 1
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_2 and len(vehicle_agents) >= 2:
+ current_view_vehicle_id = 2
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_3 and len(vehicle_agents) >= 3:
+ current_view_vehicle_id = 3
+ show_split_screen = False
+ show_top_view = False
+ elif event.key == pygame.K_s:
+ show_split_screen = True
+ show_top_view = False
+ elif event.key == pygame.K_v:
+ show_top_view = True
+ show_split_screen = False
+ elif event.key == pygame.K_ESCAPE:
+ running = False
+
+ # 清空屏幕
+ screen.fill((0, 0, 0))
+
+ if show_top_view:
+ if top_view_surface:
+ screen.blit(top_view_surface, (0, 0))
+ elif show_split_screen:
+ if len(vehicle_agents) == 1:
+ agent = vehicle_agents[0]
+ if agent.camera.image_surface:
+ surface = pygame.transform.scale(agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surface, (0, 0))
+ elif len(vehicle_agents) == 2:
+ agent1 = vehicle_agents[0]
+ agent2 = vehicle_agents[1]
+
+ if agent1.camera.image_surface:
+ surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ screen.blit(surface1, (0, 0))
+
+ if agent2.camera.image_surface:
+ surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ screen.blit(surface2, (WINDOW_WIDTH//2, 0))
+ elif len(vehicle_agents) >= 3:
+ agent1 = vehicle_agents[0]
+ agent2 = vehicle_agents[1]
+ agent3 = vehicle_agents[2]
+
+ if agent1.camera.image_surface:
+ surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surface1, (0, 0))
+
+ if agent2.camera.image_surface:
+ surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surface2, (WINDOW_WIDTH//2, 0))
+
+ if agent3.camera.image_surface:
+ surface3 = pygame.transform.scale(agent3.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
+ screen.blit(surface3, (0, WINDOW_HEIGHT//2))
+ else:
+ target_agent = None
+ for agent in vehicle_agents:
+ if agent.vehicle_id == current_view_vehicle_id:
+ target_agent = agent
+ break
+
+ if target_agent and target_agent.camera.image_surface:
+ surface = pygame.transform.scale(target_agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surface, (0, 0))
+
+ # 更新车辆状态
+ with ThreadPoolExecutor(max_workers=VEHICLE_COUNT) as executor:
+ futures = [executor.submit(agent.update) for agent in vehicle_agents]
+ for future in as_completed(futures):
+ try:
+ future.result()
+ except Exception as e:
+ print(f"车辆更新异常：{e}")
+
+ # 刷新屏幕
+ pygame.display.flip()
+ clock.tick(30)
+
+ except Exception as e:
+ print(f"仿真异常：{e}")
+ traceback.print_exc()
+ finally:
+ cleanup()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From 585f250d74b602ea92a154f04721eaed49d66548 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Mon, 22 Dec 2025 09:43:09 +0800
Subject: [PATCH 19/26] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E5=8A=9F=E8=83=BD?=
=?UTF-8?q?=E5=8C=85?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/ros/CMakeLists.txt | 206 ++++++++++++++++++
src/autonomous_driving_car/ros/package.xml | 68 ++++++
2 files changed, 274 insertions(+)
create mode 100644 src/autonomous_driving_car/ros/CMakeLists.txt
create mode 100644 src/autonomous_driving_car/ros/package.xml
diff --git a/src/autonomous_driving_car/ros/CMakeLists.txt b/src/autonomous_driving_car/ros/CMakeLists.txt
new file mode 100644
index 0000000000..1627cb3d71
--- /dev/null
+++ b/src/autonomous_driving_car/ros/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(my_robot)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES my_robot
+# CATKIN_DEPENDS roscpp rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/my_robot.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/my_robot_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_robot.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
\ No newline at end of file
diff --git a/src/autonomous_driving_car/ros/package.xml b/src/autonomous_driving_car/ros/package.xml
new file mode 100644
index 0000000000..34abba9a68
--- /dev/null
+++ b/src/autonomous_driving_car/ros/package.xml
@@ -0,0 +1,68 @@
+
+
+ my_robot
+ 0.0.0
+ The my_robot package
+
+
+
+
+ li-y
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+
+
+
+
+
+
+
+
\ No newline at end of file
From 7b57177b1f12259a421f63bcc1c5fd41d5b21399 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Mon, 22 Dec 2025 18:03:11 +0800
Subject: [PATCH 20/26] =?UTF-8?q?=E4=B8=8A=E4=BC=A0ros=E5=8A=9F=E8=83=BD?=
=?UTF-8?q?=E5=8C=85?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/ros/CMakeLists.txt | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/autonomous_driving_car/ros/CMakeLists.txt b/src/autonomous_driving_car/ros/CMakeLists.txt
index 1627cb3d71..6b3b73737f 100644
--- a/src/autonomous_driving_car/ros/CMakeLists.txt
+++ b/src/autonomous_driving_car/ros/CMakeLists.txt
@@ -22,9 +22,9 @@ find_package(catkin REQUIRED COMPONENTS
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
-################################################
+###############################################
## Declare ROS messages, services and actions ##
-################################################
+###############################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
From 53c0bd95bb41bf721b685f985371ba61a909230f Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Mon, 22 Dec 2025 18:20:39 +0800
Subject: [PATCH 21/26] =?UTF-8?q?=E5=88=A0=E9=99=A4ros=E7=9B=AE=E5=BD=95?=
=?UTF-8?q?=E4=B8=8B=E7=9A=84package.xml=E5=92=8CCMakeLists.txt=E6=96=87?=
=?UTF-8?q?=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/CAS/v4.py | 338 ------------------
src/autonomous_driving_car/README.md | 75 ++--
src/autonomous_driving_car/ros/CMakeLists.txt | 206 -----------
src/autonomous_driving_car/ros/package.xml | 68 ----
4 files changed, 20 insertions(+), 667 deletions(-)
delete mode 100644 src/autonomous_driving_car/CAS/v4.py
delete mode 100644 src/autonomous_driving_car/ros/CMakeLists.txt
delete mode 100644 src/autonomous_driving_car/ros/package.xml
diff --git a/src/autonomous_driving_car/CAS/v4.py b/src/autonomous_driving_car/CAS/v4.py
deleted file mode 100644
index 3fbfcf65c4..0000000000
--- a/src/autonomous_driving_car/CAS/v4.py
+++ /dev/null
@@ -1,338 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-"""
-CARLA 弯道自适应转向：根据弯道大小动态调整转弯幅度
-"""
-
-import sys
-import os
-import carla
-import numpy as np
-import math
-import pygame
-import traceback
-
-# ===================== 全局配置（动态参数）=====================
-# CARLA连接
-CARLA_HOST = "localhost"
-CARLA_PORT = 2000
-CARLA_TIMEOUT = 10.0
-
-# 车辆配置
-VEHICLE_MODEL = "vehicle.tesla.model3"
-VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距（米）
-VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移（正数）
-
-# 转向控制（动态参数基准）
-LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离
-LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离
-STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益
-STEER_GAIN_CURVE = 1.0 # 急弯转向增益
-STEER_DEADZONE = 0.05 # 转向死区
-STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数
-STEER_DIR_COEFF = 1.0 # 转向方向校准
-MAX_STEER = 1.0 # 最大转向角
-
-# 弯道等级划分（方向变化量阈值）
-DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值
-DIR_CHANGE_SHARP = 0.08 # 急弯阈值
-
-# 速度控制
-BASE_SPEED = 28.0 # 直道基础速度
-CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数（更明显的速度衰减）
-PID_KP = 0.15
-PID_KI = 0.01
-PID_KD = 0.01
-
-# 相机
-CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
-CAMERA_WIDTH = 800
-CAMERA_HEIGHT = 600
-CAMERA_FOV = 90
-
-# ===================== 纯追踪控制器（动态适配）=====================
-class AdaptivePurePursuit:
- def __init__(self, wheelbase):
- self.wheelbase = wheelbase
- self.last_steer = 0.0 # 上一帧转向角
- self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离
-
- def calculate_steer(self, vehicle_transform, target_point, dir_change):
- """
- 自适应纯追踪计算（根据弯道大小调整参数）
- :param vehicle_transform: 车辆变换
- :param target_point: 目标点（carla.Location）
- :param dir_change: 方向变化量（弯道大小）
- :return: 自适应后的转向角
- """
- # 1. 获取车辆后轴位置
- forward_vec = vehicle_transform.get_forward_vector()
- rear_axle_loc = carla.Location(
- x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
- y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
- z=vehicle_transform.location.z
- )
-
- # 2. 转换到车辆坐标系
- dx = target_point.x - rear_axle_loc.x
- dy = target_point.y - rear_axle_loc.y
- yaw = math.radians(vehicle_transform.rotation.yaw)
-
- dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
- dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
-
- # 3. 动态计算转向增益（根据弯道大小线性插值）
- # 方向变化量越大，增益越高
- steer_gain = np.interp(
- dir_change,
- [0, DIR_CHANGE_SHARP],
- [STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
- )
- steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
-
- # 4. 纯追踪核心计算
- if dx_vehicle < 0.1:
- steer = self.last_steer
- else:
- # 纯追踪公式 + 弯道大小系数（dir_change*2 放大差异）
- steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2)
- steer = steer_rad / math.pi
-
- # 应用增益和方向校准
- steer *= steer_gain * STEER_DIR_COEFF
-
- # 5. 应用死区
- if abs(steer) < STEER_DEADZONE:
- steer = 0.0
-
- # 6. 低通滤波（平滑）
- steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
-
- # 7. 限制范围
- steer = np.clip(steer, -MAX_STEER, MAX_STEER)
-
- # 8. 更新状态
- self.last_steer = steer
-
- return steer
-
- def get_adaptive_lookahead(self, dir_change):
- """
- 动态获取预瞄距离（根据弯道大小线性插值）
- :param dir_change: 方向变化量
- :return: 自适应预瞄距离
- """
- lookahead_dist = np.interp(
- dir_change,
- [0, DIR_CHANGE_SHARP],
- [LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
- )
- lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
- self.last_lookahead = lookahead_dist
- return lookahead_dist
-
-# ===================== 速度控制器（动态调整）=====================
-class SpeedController:
- def __init__(self, kp, ki, kd):
- self.kp = kp
- self.ki = ki
- self.kd = kd
- self.last_error = 0.0
- self.integral = 0.0
-
- def calculate(self, target_speed, current_speed):
- """PID速度控制"""
- error = target_speed - current_speed
-
- p = self.kp * error
- self.integral += self.ki * error
- self.integral = np.clip(self.integral, -1.0, 1.0)
- i = self.integral
- d = self.kd * (error - self.last_error)
- self.last_error = error
-
- output = p + i + d
- return np.clip(output, 0.0, 1.0)
-
-# ===================== 辅助函数：计算方向变化（替代曲率）=====================
-def calculate_dir_change(current_wp):
- """
- 计算相邻Waypoint的方向变化量（更精细的计算，增加采样点）
- :param current_wp: 当前Waypoint
- :return: 方向变化的绝对值之和，弯道等级（0=直道，1=缓弯，2=急弯）
- """
- waypoints = [current_wp]
- # 增加采样点到5个，更准确的判断弯道大小
- for i in range(5):
- next_wps = waypoints[-1].next(1.0)
- if next_wps:
- waypoints.append(next_wps[0])
- else:
- break
-
- if len(waypoints) < 4:
- return 0.0, 0
-
- # 计算每个相邻点的方向
- dirs = []
- for i in range(1, len(waypoints)):
- wp_prev = waypoints[i-1]
- wp_curr = waypoints[i]
- dir_rad = math.atan2(
- wp_curr.transform.location.y - wp_prev.transform.location.y,
- wp_curr.transform.location.x - wp_prev.transform.location.x
- )
- dirs.append(dir_rad)
-
- # 计算方向变化的绝对值之和（放大差异）
- dir_change = 0.0
- for i in range(1, len(dirs)):
- dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异
-
- # 划分弯道等级
- if dir_change < DIR_CHANGE_GENTLE:
- curve_level = 0 # 直道
- elif dir_change < DIR_CHANGE_SHARP:
- curve_level = 1 # 缓弯
- else:
- curve_level = 2 # 急弯
-
- return dir_change, curve_level
-
-# ===================== 相机管理器 =====================
-class CameraManager:
- def __init__(self, world, vehicle, display):
- self.world = world
- self.vehicle = vehicle
- self.display = display
- self.camera = None
- self._create_camera()
-
- def _create_camera(self):
- bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
- bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
- bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
- bp.set_attribute("fov", str(CAMERA_FOV))
- self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
- self.camera.listen(self._on_image)
-
- def _on_image(self, image):
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3]
- array = array[:, :, ::-1].swapaxes(0, 1)
- self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
- pygame.display.flip()
-
- def destroy(self):
- if self.camera:
- self.camera.stop()
- self.camera.destroy()
-
-# ===================== 主函数（核心逻辑）=====================
-def main():
- pygame.init()
- display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
- pygame.display.set_caption("CARLA 弯道自适应转向（最终版）")
-
- client = None
- world = None
- vehicle = None
- camera_manager = None
- pp_controller = None
- speed_controller = None
-
- try:
- # 1. 连接CARLA并初始化
- client = carla.Client(CARLA_HOST, CARLA_PORT)
- client.set_timeout(CARLA_TIMEOUT)
- world = client.get_world()
- map = world.get_map()
-
- # 清理现有车辆
- for actor in world.get_actors().filter("vehicle.*"):
- actor.destroy()
-
- # 生成车辆
- vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
- spawn_points = map.get_spawn_points()
- vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])
- print(f"车辆生成成功：{vehicle.type_id}")
-
- # 初始化控制器
- pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
- speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
- camera_manager = CameraManager(world, vehicle, display)
-
- # 2. 主循环
- print("仿真启动，按ESC退出...")
- clock = pygame.time.Clock()
- running = True
-
- while running:
- # 事件处理
- for event in pygame.event.get():
- if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
- running = False
-
- # 3. 获取关键数据
- vehicle_transform = vehicle.get_transform()
- vehicle_vel = vehicle.get_velocity()
- current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h
-
- # 获取当前Waypoint
- current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True)
-
- # 计算方向变化量和弯道等级
- dir_change, curve_level = calculate_dir_change(current_wp)
-
- # 动态获取预瞄距离
- lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)
-
- # 获取目标Waypoint（动态预瞄距离）
- target_wps = current_wp.next(lookahead_dist)
- target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
-
- # 4. 计算目标速度（根据弯道等级调整）
- curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数
- speed_factor = curve_speed_factors[min(curve_level, 2)]
- target_speed = BASE_SPEED * speed_factor
- target_speed = max(10.0, target_speed)
-
- # 5. 计算转向角（传入方向变化量，实现自适应）
- steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
-
- # 6. 计算油门/刹车
- throttle = speed_controller.calculate(target_speed, current_speed)
- brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0
-
- # 7. 应用控制
- control = carla.VehicleControl()
- control.steer = steer
- control.throttle = throttle
- control.brake = brake
- control.hand_brake = False
- vehicle.apply_control(control)
-
- # 8. 打印状态（调试用，显示弯道等级和动态参数）
- curve_names = ["直道", "缓弯", "急弯"]
- print(f"速度：{current_speed:.1f}km/h | 目标速度：{target_speed:.1f}km/h | 弯道：{curve_names[curve_level]} | 预瞄：{lookahead_dist:.1f}m | 转向：{steer:.3f}")
-
- # 控制帧率
- clock.tick(30)
-
- except Exception as e:
- print(f"错误：{e}")
- traceback.print_exc()
-
- finally:
- # 清理资源
- print("清理资源...")
- if camera_manager:
- camera_manager.destroy()
- if vehicle:
- vehicle.destroy()
- pygame.quit()
- print("仿真结束")
-
-if __name__ == "__main__":
- main()
\ No newline at end of file
diff --git a/src/autonomous_driving_car/README.md b/src/autonomous_driving_car/README.md
index 0bb5ae9ee2..019a9c1439 100644
--- a/src/autonomous_driving_car/README.md
+++ b/src/autonomous_driving_car/README.md
@@ -1,62 +1,32 @@
CARLA 模拟器和自动驾驶基础算法学习
-
-
-\# 无人驾驶汽车项目（基于CARLA模拟器）
-
-\## 项目简介
-
+# 无人驾驶汽车项目（基于CARLA模拟器）
+## 项目简介
本项目是基于CARLA开源仿真平台、Python和PyCharm开发的无人驾驶仿真系统，融合计算机视觉、路径规划与控制技术，实现虚拟场景中车辆的自主导航、避障与路径跟踪功能，适用于自动驾驶入门实践。
+## 核心功能
+- 路径规划：A*算法、RRT/RRT*算法实现起点到终点路径生成
+- 障碍物检测：YOLOv8+OpenCV实时识别目标，激光雷达点云聚类定位
+- 车辆控制：PID控制器实现转向、速度精准控制
+- CARLA交互：加载场景、获取传感器数据（摄像头/激光雷达等）、发送控制指令
+- 实时可视化：PyGame显示场景、车辆状态与检测结果
-
-\## 核心功能
-
-\- 路径规划：A\*算法、RRT/RRT\*算法实现起点到终点路径生成
-
-\- 障碍物检测：YOLOv8+OpenCV实时识别目标，激光雷达点云聚类定位
-
-\- 车辆控制：PID控制器实现转向、速度精准控制
-
-\- CARLA交互：加载场景、获取传感器数据（摄像头/激光雷达等）、发送控制指令
-
-\- 实时可视化：PyGame显示场景、车辆状态与检测结果
-
-
-
-\## 技术栈
-
+## 技术栈
| 类别 | 具体技术/工具 |
-
|--------------|---------------------------------------|
-
| 开发环境 | PyCharm Community Edition 2024+、Windows/Linux |
-
| 核心语言 | Python 3.8+ |
-
| 仿真平台 | CARLA 0.9.15/0.9.16、CARLA Python API |
-
| 计算机视觉 | OpenCV、NumPy、YOLOv8（Ultralytics） |
-
-| 路径规划 | A\*、RRT/RRT\*算法 |
-
+| 路径规划 | A*、RRT/RRT*算法 |
| 控制理论 | PID控制器 |
-
| 可视化与数据 | PyGame、Matplotlib、Pandas |
-
-
-\## 快速开始
-
-1\. 克隆项目到PyCharm，创建Python 3.8+虚拟环境
-
-2\. 安装依赖：`pip install -r requirements.txt`（核心依赖：carla、opencv-python、ultralytics、numpy、pygame）
-
-3\. 启动CARLA模拟器（运行`CarlaUE4.exe`/`CarlaUE4.sh`）
-
-4\. 运行主程序：`python main.py`，自动连接CARLA并自主导航
-
-
+## 快速开始
+1. 克隆项目到PyCharm，创建Python 3.8+虚拟环境
+2. 安装依赖：`pip install -r requirements.txt`（核心依赖：carla、opencv-python、ultralytics、numpy、pygame）
+3. 启动CARLA模拟器（运行`CarlaUE4.exe`/`CarlaUE4.sh`）
+4. 运行主程序：`python main.py`，自动连接CARLA并启动自主导航
\## 项目结构
@@ -65,14 +35,9 @@ autonomous_driving_car/
├── MCP/ # 主控制与模块集成：包含感知（检测）、规划（路径）、控制（PID）的核心逻辑
├── main.py # 项目入口：启动CARLA连接、调用MCP与FPV模块
└── README.md # 项目说明文档
+```
-
-
-\## 常见问题
-
-\- CARLA连接失败：确保模拟器已启动，Python API版本与CARLA一致
-
-\- 检测速度慢：使用YOLOv8n轻量模型，或启用GPU加速
-
-\- 控制不稳定：调整PID参数或增加路径平滑处理
-
+## 常见问题
+- CARLA连接失败：确保模拟器已启动，Python API版本与CARLA一致
+- 检测速度慢：使用YOLOv8n轻量模型，或启用GPU加速
+- 控制不稳定：调整PID参数或增加路径平滑处理
\ No newline at end of file
diff --git a/src/autonomous_driving_car/ros/CMakeLists.txt b/src/autonomous_driving_car/ros/CMakeLists.txt
deleted file mode 100644
index 6b3b73737f..0000000000
--- a/src/autonomous_driving_car/ros/CMakeLists.txt
+++ /dev/null
@@ -1,206 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(my_robot)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- rospy
- std_msgs
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-###############################################
-## Declare ROS messages, services and actions ##
-###############################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-## * add a build_depend tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-## * add "message_generation" and every package in MSG_DEP_SET to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * add "message_runtime" and every package in MSG_DEP_SET to
-## catkin_package(CATKIN_DEPENDS ...)
-## * uncomment the add_*_files sections below as needed
-## and list every .msg/.srv/.action file to be processed
-## * uncomment the generate_messages entry below
-## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-# FILES
-# Action1.action
-# Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-# INCLUDE_DIRS include
-# LIBRARIES my_robot
-# CATKIN_DEPENDS roscpp rospy std_msgs
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/my_robot.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/my_robot_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_robot.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
\ No newline at end of file
diff --git a/src/autonomous_driving_car/ros/package.xml b/src/autonomous_driving_car/ros/package.xml
deleted file mode 100644
index 34abba9a68..0000000000
--- a/src/autonomous_driving_car/ros/package.xml
+++ /dev/null
@@ -1,68 +0,0 @@
-
-
- my_robot
- 0.0.0
- The my_robot package
-
-
-
-
- li-y
-
-
-
-
-
- TODO
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- roscpp
- rospy
- std_msgs
- roscpp
- rospy
- std_msgs
- roscpp
- rospy
- std_msgs
-
-
-
-
-
-
-
-
\ No newline at end of file
From 684015ac911ae40575331a6609827f820b4a3625 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Mon, 22 Dec 2025 18:25:54 +0800
Subject: [PATCH 22/26] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E5=8A=9F=E8=83=BD?=
=?UTF-8?q?=E5=8C=85?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/ros/CMakeLists.txt | 206 ++++++++++++++++++
src/autonomous_driving_car/ros/package.xml | 68 ++++++
2 files changed, 274 insertions(+)
create mode 100644 src/autonomous_driving_car/ros/CMakeLists.txt
create mode 100644 src/autonomous_driving_car/ros/package.xml
diff --git a/src/autonomous_driving_car/ros/CMakeLists.txt b/src/autonomous_driving_car/ros/CMakeLists.txt
new file mode 100644
index 0000000000..6b3b73737f
--- /dev/null
+++ b/src/autonomous_driving_car/ros/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(my_robot)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+###############################################
+## Declare ROS messages, services and actions ##
+###############################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES my_robot
+# CATKIN_DEPENDS roscpp rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/my_robot.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/my_robot_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_robot.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
\ No newline at end of file
diff --git a/src/autonomous_driving_car/ros/package.xml b/src/autonomous_driving_car/ros/package.xml
new file mode 100644
index 0000000000..34abba9a68
--- /dev/null
+++ b/src/autonomous_driving_car/ros/package.xml
@@ -0,0 +1,68 @@
+
+
+ my_robot
+ 0.0.0
+ The my_robot package
+
+
+
+
+ li-y
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+
+
+
+
+
+
+
+
\ No newline at end of file
From c5bee8a546d876b786e430d6dce83454278d5972 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Mon, 22 Dec 2025 19:48:01 +0800
Subject: [PATCH 23/26] =?UTF-8?q?=E6=9B=B4=E6=96=B0README.md?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/README.md | 61 +++++++++++++++++-----------
1 file changed, 38 insertions(+), 23 deletions(-)
diff --git a/src/autonomous_driving_car/README.md b/src/autonomous_driving_car/README.md
index 019a9c1439..022d8eb56a 100644
--- a/src/autonomous_driving_car/README.md
+++ b/src/autonomous_driving_car/README.md
@@ -1,43 +1,58 @@
-CARLA 模拟器和自动驾驶基础算法学习
-
-# 无人驾驶汽车项目（基于CARLA模拟器）
+# 无人驾驶汽车项目（基于CARLA 0.9.10+ROS）
## 项目简介
-本项目是基于CARLA开源仿真平台、Python和PyCharm开发的无人驾驶仿真系统，融合计算机视觉、路径规划与控制技术，实现虚拟场景中车辆的自主导航、避障与路径跟踪功能，适用于自动驾驶入门实践。
+本项目是基于**CARLA 0.9.10**开源仿真平台、Python 3.8+及ROS机器人操作系统开发的无人驾驶仿真系统，聚焦自动驾驶“感知-规划-控制”核心流程的落地实现。通过CARLA 0.9.10获取高保真的虚拟交通场景与传感器数据，结合计算机视觉、经典路径规划算法和PID控制技术，实现车辆自主导航、避障与路径跟踪，并利用ROS完成模块间分布式通信，同时通过FPV模块实现第一视角实时可视化，是自动驾驶入门实践的轻量化项目。
## 核心功能
-- 路径规划：A*算法、RRT/RRT*算法实现起点到终点路径生成
-- 障碍物检测：YOLOv8+OpenCV实时识别目标，激光雷达点云聚类定位
-- 车辆控制：PID控制器实现转向、速度精准控制
-- CARLA交互：加载场景、获取传感器数据（摄像头/激光雷达等）、发送控制指令
-- 实时可视化：PyGame显示场景、车辆状态与检测结果
+1. **CARLA 0.9.10仿真交互**：支持加载城市场景、创建车辆与多传感器（摄像头、激光雷达、GPS/IMU），实时获取传感器数据并下发车辆控制指令，完美适配CARLA 0.9.10的API特性与车辆动力学模型。
+2. **环境感知**：采用YOLOv8+OpenCV实现车辆、行人、障碍物的实时目标检测，结合激光雷达点云聚类完成障碍物的精准定位与距离测算。
+3. **路径规划**：基于A*算法生成全局最优路径，RRT*算法实现动态避障的局部路径规划，输出适配车辆行驶的平滑参考轨迹。
+4. **车辆控制**：通过PID控制器实现转向、速度与制动的精准控制，高效跟踪规划路径，保证车辆行驶的稳定性。
+5. **模块化通信与可视化**：ROS模块实现感知、规划、控制模块的话题订阅与发布，支持分布式调试；FPV模块实时渲染车辆第一视角画面及检测结果。
## 技术栈
| 类别 | 具体技术/工具 |
|--------------|---------------------------------------|
| 开发环境 | PyCharm Community Edition 2024+、Windows/Linux |
| 核心语言 | Python 3.8+ |
-| 仿真平台 | CARLA 0.9.15/0.9.16、CARLA Python API |
+| 仿真平台 | CARLA 0.9.10、CARLA Python API |
| 计算机视觉 | OpenCV、NumPy、YOLOv8（Ultralytics） |
-| 路径规划 | A*、RRT/RRT*算法 |
+| 路径规划 | A*、RRT*算法 |
| 控制理论 | PID控制器 |
-| 可视化与数据 | PyGame、Matplotlib、Pandas |
+| 通信框架 | ROS（Noetic/Melodic）、rospy |
+| 可视化工具 | PyGame、Matplotlib |
## 快速开始
-1. 克隆项目到PyCharm，创建Python 3.8+虚拟环境
-2. 安装依赖：`pip install -r requirements.txt`（核心依赖：carla、opencv-python、ultralytics、numpy、pygame）
-3. 启动CARLA模拟器（运行`CarlaUE4.exe`/`CarlaUE4.sh`）
-4. 运行主程序：`python main.py`，自动连接CARLA并启动自主导航
+### 1. 环境准备
+1. 安装Python 3.8+，创建并激活虚拟环境；
+2. 安装项目依赖：`pip install -r requirements.txt`（核心依赖：`carla==0.9.10`、opencv-python、ultralytics、numpy、pygame、rospy）；
+3. 配置ROS环境（可选）：安装对应版本ROS并初始化工作空间，将项目中`ros`文件夹链接到ROS工作空间`src`目录；
+4. 启动CARLA 0.9.10模拟器：运行`CarlaUE4.exe`（Windows）或`CarlaUE4.sh`（Linux）。
+
+### 2. 项目运行
+#### （1）无ROS模式
+直接运行主程序，自动连接CARLA并启动全流程功能：
+```bash
+python main.py
+```
-\## 项目结构
+#### （2）ROS模式
+1. 启动ROS核心：`roscore`；
+2. 依次启动CARLA-ROS桥接节点、感知/规划/控制节点及FPV可视化节点（具体命令见项目内注释）；
+3. 各节点通过ROS话题实现数据交互，完成车辆自主导航。
+## 项目结构
+```
autonomous_driving_car/
-├── FPV/ # 第一视角可视化模块：负责车辆摄像头视角、检测结果的实时显示
-├── MCP/ # 主控制与模块集成：包含感知（检测）、规划（路径）、控制（PID）的核心逻辑
-├── main.py # 项目入口：启动CARLA连接、调用MCP与FPV模块
+├── FPV/ # 第一视角可视化模块
+├── MCP/ # 主控制模块（集成感知、规划、控制核心逻辑）
+├── ros/ # ROS通信模块
+├── main.py # 项目入口
└── README.md # 项目说明文档
```
## 常见问题
-- CARLA连接失败：确保模拟器已启动，Python API版本与CARLA一致
-- 检测速度慢：使用YOLOv8n轻量模型，或启用GPU加速
-- 控制不稳定：调整PID参数或增加路径平滑处理
\ No newline at end of file
+1. **CARLA 0.9.10连接失败**：确保模拟器已启动，且Python安装的`carla`版本为0.9.10（可通过`pip show carla`查看版本）；
+2. **ROS节点无数据交互**：检查`roscore`是否启动、话题名称是否一致、ROS环境变量是否正确`source`；
+3. **YOLO检测速度慢**：使用YOLOv8n轻量级模型，或启用GPU加速（需安装CUDA）；
+4. **车辆控制不稳定**：调整PID控制器的P/I/D参数，或对规划路径进行平滑处理；
+5. **CARLA 0.9.10传感器数据异常**：确认传感器挂载位置与参数设置符合CARLA 0.9.10的Actor生成规则。
\ No newline at end of file
From 7638494070ab3e4f3886b20ccd539d2d3807c5b2 Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Wed, 24 Dec 2025 00:16:08 +0800
Subject: [PATCH 24/26] =?UTF-8?q?=E6=B7=BB=E5=8A=A0ACC=E8=B7=9F=E8=BD=A6?=
=?UTF-8?q?=E7=9B=B8=E5=85=B3=E7=9A=84=E5=85=A8=E5=B1=80=E9=85=8D=E7=BD=AE?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 74 ++++++++++++++++++++++++------
1 file changed, 59 insertions(+), 15 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 533eccd517..18e0a0ed08 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
-CARLA 多车辆协同控制版：修复出生点索引越界问题
+CARLA 多车辆协同控制版：V1.0 增强ACC跟车+紧急避障
"""
import sys
@@ -52,6 +52,12 @@
PID_KI = 0.01
PID_KD = 0.02
+# ACC跟车配置（新增）
+SAFE_TIME_GAP = 1.5 # 安全时距（秒）
+MIN_SAFE_DISTANCE = 5.0 # 最小安全距离（米）
+EMERGENCY_DECEL_RATE = 5.0 # 紧急制动减速度（km/h/帧）
+LEAD_BRAKE_THRESHOLD = -10.0 # 前车急刹加速度阈值（km/h/s）
+
# 交通规则配置
TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
TRAFFIC_LIGHT_DETECTION_RANGE = 50.0
@@ -72,12 +78,13 @@
# 全局变量
current_view_vehicle_id = 1
vehicle_agents = []
+COLLISION_FLAG = {} # 碰撞标志（新增）
# 日志配置
logging.basicConfig(
level=logging.INFO,
format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
- handlers=[logging.FileHandler("multi_vehicle_simulation.log"), logging.StreamHandler()]
+ handlers=[logging.FileHandler("multi_vehicle_simulation_v1.log"), logging.StreamHandler()]
)
# ===================== 核心工具函数 ======================
@@ -326,6 +333,11 @@ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_spee
self.logger = logging.getLogger(__name__)
self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+ # 新增：ACC跟车相关属性
+ self.last_lead_speed = 0.0 # 前车上次速度
+ self.stuck_count = 0 # 卡死计数
+ COLLISION_FLAG[self.vehicle_id] = False # 初始化碰撞标志
+
# 生成车辆
self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
if self.vehicle_bp.has_attribute("color"):
@@ -400,17 +412,42 @@ def update(self):
base_target_speed = self.base_speed * speed_factor
base_target_speed = max(8.0, base_target_speed)
- # 跟车控制
- global vehicle_agents
+ # ========== 新增：精细化ACC跟车+紧急避障逻辑 ==========
if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
try:
- lead_vehicle = vehicle_agents[self.vehicle_id - 2].vehicle
+ lead_agent = vehicle_agents[self.vehicle_id - 2]
+ lead_vehicle = lead_agent.vehicle
lead_vehicle_transform = lead_vehicle.get_transform()
+
+ # 计算前车速度和加速度
+ lead_vel = lead_vehicle.get_velocity()
+ lead_speed = math.hypot(lead_vel.x, lead_vel.y) * 3.6
+ lead_acc = (lead_speed - lead_agent.last_lead_speed) / 0.03 # 30Hz刷新率，计算加速度
+ lead_agent.last_lead_speed = lead_speed # 更新前车上次速度
+
+ # 计算安全跟车距离（安全时距+最小安全距）
+ safe_dist = (current_speed / 3.6) * SAFE_TIME_GAP + MIN_SAFE_DISTANCE
dist_to_lead = vehicle_transform.location.distance(lead_vehicle_transform.location)
- if dist_to_lead < 15.0:
- base_target_speed = max(5.0, base_target_speed * 0.5)
- except:
- pass
+
+ # 动态调整目标速度
+ if dist_to_lead < safe_dist - 2:
+ # 过近：减速至前车速度-2（不低于5km/h）
+ base_target_speed = max(5.0, lead_speed - 2)
+ elif dist_to_lead > safe_dist + 2:
+ # 过远：加速至前车速度+2（不超基础速度）
+ base_target_speed = min(self.base_speed * speed_factor, lead_speed + 2)
+ else:
+ # 安全距离：与前车速度同步
+ base_target_speed = lead_speed
+
+ # 紧急避障：前车急刹（加速度<阈值）
+ if lead_acc < LEAD_BRAKE_THRESHOLD:
+ base_target_speed = max(0.0, current_speed - EMERGENCY_DECEL_RATE)
+ self.logger.warning(f"前车急刹（加速度{lead_acc:.1f}km/h/s）！紧急减速至{base_target_speed:.1f}km/h")
+
+ except Exception as e:
+ self.logger.warning(f"ACC跟车计算异常：{e}")
+ # ========== ACC跟车逻辑结束 ==========
# 交通灯处理
target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
@@ -426,6 +463,12 @@ def update(self):
throttle = 0.0
brake = 1.0
+ # 新增：碰撞后紧急停车
+ if COLLISION_FLAG.get(self.vehicle_id, False):
+ throttle = 0.0
+ brake = 1.0
+ self.logger.error("检测到碰撞，紧急停车！")
+
# 应用控制
control = carla.VehicleControl()
control.steer = steer
@@ -433,10 +476,11 @@ def update(self):
control.brake = brake
self.vehicle.apply_control(control)
- # 日志输出
+ # 日志输出（新增ACC相关信息）
self.logger.info(
f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | "
- f"弯道：{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态：{traffic_light_status}"
+ f"弯道：{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态：{traffic_light_status} | "
+ f"ACC：{'激活' if self.vehicle_id>1 else '未激活'}"
)
return True
@@ -521,10 +565,10 @@ def calculate(self, target_speed, current_speed):
p = self.kp * error
self.integral += self.ki * error
self.integral = np.clip(self.integral, -1.0, 1.0)
- i = self.integral
d = self.kd * (error - self.last_error)
self.last_error = error
- return np.clip(p + i + d, 0.0, 1.0)
+ # 修复：将 i 改为 self.integral
+ return np.clip(p + self.integral + d, 0.0, 1.0)
class TrafficLightManager:
def __init__(self, vehicle_id):
@@ -681,7 +725,7 @@ def main():
global current_view_vehicle_id, vehicle_agents
pygame.init()
screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
- pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
+ pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- V1.0 ACC跟车 - 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
client = None
world = None
@@ -777,7 +821,7 @@ def cleanup():
if len(vehicle_agents) == 0:
raise RuntimeError("无车辆生成成功，仿真终止")
- print(f"\n共生成{len(vehicle_agents)}辆车辆！")
+ print(f"\n共生成{len(vehicle_agents)}辆车辆！V1.0 ACC跟车功能已启用")
# 创建全局俯视相机
try:
From 7f13466d586da80ac72cd2cb0762a7902c9108af Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Wed, 24 Dec 2025 19:20:23 +0800
Subject: [PATCH 25/26] =?UTF-8?q?=E5=AE=8C=E5=96=84=E6=84=9F=E7=9F=A5?=
=?UTF-8?q?=E6=A8=A1=E5=9D=97?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 222 ++++++++++++++++++++++-------
1 file changed, 171 insertions(+), 51 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 18e0a0ed08..45ddb3e1b7 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
-CARLA 多车辆协同控制版：V1.0 增强ACC跟车+紧急避障
+CARLA 多车辆协同控制版：V2.0 增强感知（LiDAR+碰撞检测+障碍物避障）
"""
import sys
@@ -52,12 +52,22 @@
PID_KI = 0.01
PID_KD = 0.02
-# ACC跟车配置（新增）
+# ACC跟车配置（V1.0保留）
SAFE_TIME_GAP = 1.5 # 安全时距（秒）
MIN_SAFE_DISTANCE = 5.0 # 最小安全距离（米）
EMERGENCY_DECEL_RATE = 5.0 # 紧急制动减速度（km/h/帧）
LEAD_BRAKE_THRESHOLD = -10.0 # 前车急刹加速度阈值（km/h/s）
+# LiDAR与障碍物检测配置（V2.0新增）
+LIDAR_RANGE = 30.0 # LiDAR检测范围（米）
+LIDAR_POINTS_PER_SECOND = 100000 # LiDAR点云密度
+LIDAR_ROTATION_FREQ = 30 # LiDAR刷新率（Hz）
+OBSTACLE_DETECTION_WIDTH = 2.0 # 检测宽度（左右各2米）
+OBSTACLE_MIN_HEIGHT = 0.5 # 障碍物最小高度（过滤地面）
+OBSTACLE_WARNING_DIST = 8.0 # 障碍物预警距离（米）
+OBSTACLE_EMERGENCY_DIST = 5.0 # 障碍物紧急制动距离（米）
+OBSTACLE_DECEL_RATE = 8.0 # 障碍物制动减速度（km/h/帧）
+
# 交通规则配置
TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
TRAFFIC_LIGHT_DETECTION_RANGE = 50.0
@@ -78,13 +88,14 @@
# 全局变量
current_view_vehicle_id = 1
vehicle_agents = []
-COLLISION_FLAG = {} # 碰撞标志（新增）
+COLLISION_FLAG = {} # 碰撞标志（V2.0扩展）
+OBSTACLE_FLAG = {} # 障碍物标志（V2.0新增）
# 日志配置
logging.basicConfig(
level=logging.INFO,
format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
- handlers=[logging.FileHandler("multi_vehicle_simulation_v1.log"), logging.StreamHandler()]
+ handlers=[logging.FileHandler("multi_vehicle_simulation_v2.log"), logging.StreamHandler()]
)
# ===================== 核心工具函数 ======================
@@ -282,20 +293,32 @@ def check_spawn_collision(world, spawn_point, radius=3.0):
return True
-# ===================== 相机管理类（多车辆）=====================
-class VehicleCamera:
+# ===================== 传感器管理类（V2.0重构：相机+LiDAR+碰撞）=====================
+class VehicleSensors:
def __init__(self, world, vehicle, vehicle_id):
self.world = world
self.vehicle = vehicle
self.vehicle_id = vehicle_id
+ self.logger = logging.getLogger(__name__)
+ self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+
+ # 传感器实例
self.camera = None
+ self.lidar = None
+ self.collision_sensor = None
+
+ # 数据存储
self.image_surface = None
+ self.obstacle_distances = [] # 前方障碍物距离列表
+ self.last_obstacle_dist = float('inf') # 最近障碍物距离
- # 创建相机传感器
+ # 创建所有传感器
self._create_camera()
+ self._create_lidar()
+ self._create_collision_sensor()
def _create_camera(self):
- # 加载相机蓝图
+ """创建RGB相机传感器"""
camera_bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
camera_bp.set_attribute("image_size_x", str(640))
camera_bp.set_attribute("image_size_y", str(360))
@@ -303,25 +326,104 @@ def _create_camera(self):
# 生成相机（附加到车辆）
self.camera = self.world.spawn_actor(camera_bp, CAMERA_POS, attach_to=self.vehicle)
-
# 注册图像回调函数
self.camera.listen(self._on_image)
+ def _create_lidar(self):
+ """创建LiDAR传感器"""
+ lidar_bp = self.world.get_blueprint_library().find("sensor.lidar.ray_cast")
+ # 设置LiDAR参数
+ lidar_bp.set_attribute("range", str(LIDAR_RANGE))
+ lidar_bp.set_attribute("points_per_second", str(LIDAR_POINTS_PER_SECOND))
+ lidar_bp.set_attribute("rotation_frequency", str(LIDAR_ROTATION_FREQ))
+ lidar_bp.set_attribute("channels", "32") # 32线LiDAR
+ lidar_bp.set_attribute("upper_fov", "15")
+ lidar_bp.set_attribute("lower_fov", "-25")
+ lidar_bp.set_attribute("points_per_second", str(LIDAR_POINTS_PER_SECOND))
+
+ # LiDAR挂载位置（车顶）
+ lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.0))
+ self.lidar = self.world.spawn_actor(lidar_bp, lidar_transform, attach_to=self.vehicle)
+ # 注册LiDAR回调函数
+ self.lidar.listen(self._on_lidar_data)
+
+ def _create_collision_sensor(self):
+ """创建碰撞传感器"""
+ collision_bp = self.world.get_blueprint_library().find("sensor.other.collision")
+ self.collision_sensor = self.world.spawn_actor(collision_bp, carla.Transform(), attach_to=self.vehicle)
+ # 注册碰撞回调函数
+ self.collision_sensor.listen(self._on_collision)
+
def _on_image(self, image):
- # 将CARLA图像转换为Pygame Surface
+ """相机图像回调：转换为Pygame Surface"""
array = np.frombuffer(image.raw_data, dtype=np.uint8)
array = array.reshape((image.height, image.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
array = np.swapaxes(array, 0, 1)
-
- # 存储为Pygame Surface
self.image_surface = pygame.surfarray.make_surface(array)
+ def _on_lidar_data(self, data):
+ """LiDAR点云回调：解析并提取前方障碍物"""
+ try:
+ # 将点云数据转换为numpy数组 (x, y, z, intensity)
+ points = np.frombuffer(data.raw_data, dtype=np.float32).reshape(-1, 4)
+
+ # 过滤条件：
+ # 1. 前方（x>0）
+ # 2. 左右范围内（|y| < 检测宽度）
+ # 3. 非地面（z > 最小高度）
+ front_obstacle_points = points[
+ (points[:, 0] > 0) &
+ (np.abs(points[:, 1]) < OBSTACLE_DETECTION_WIDTH) &
+ (points[:, 2] > OBSTACLE_MIN_HEIGHT)
+ ]
+
+ # 计算最近障碍物距离
+ if len(front_obstacle_points) > 0:
+ self.last_obstacle_dist = np.min(front_obstacle_points[:, 0])
+ self.obstacle_distances.append(self.last_obstacle_dist)
+ # 只保留最近10帧数据（平滑滤波）
+ if len(self.obstacle_distances) > 10:
+ self.obstacle_distances.pop(0)
+ else:
+ self.last_obstacle_dist = float('inf')
+ self.obstacle_distances.clear()
+
+ except Exception as e:
+ self.logger.error(f"LiDAR数据解析失败：{e}")
+
+ def _on_collision(self, event):
+ """碰撞回调：记录碰撞信息并触发紧急停车"""
+ try:
+ collision_actor_type = event.other_actor.type_id
+ collision_location = event.transform.location
+ self.logger.error(
+ f"发生碰撞！碰撞对象：{collision_actor_type} | 碰撞位置：({collision_location.x:.1f}, {collision_location.y:.1f})"
+ )
+ # 设置碰撞标志
+ global COLLISION_FLAG
+ COLLISION_FLAG[self.vehicle_id] = True
+ except Exception as e:
+ self.logger.error(f"碰撞检测回调失败：{e}")
+
+ def get_average_obstacle_distance(self):
+ """获取平滑后的障碍物距离"""
+ if len(self.obstacle_distances) == 0:
+ return float('inf')
+ return np.mean(self.obstacle_distances)
+
def destroy(self):
- if self.camera:
- self.camera.stop()
- self.camera.destroy()
+ """销毁所有传感器"""
+ sensors = [self.camera, self.lidar, self.collision_sensor]
+ for sensor in sensors:
+ if sensor:
+ try:
+ sensor.stop()
+ sensor.destroy()
+ except:
+ pass
+ self.logger.info("所有传感器已销毁")
# ===================== 车辆控制类 ======================
class VehicleAgent:
@@ -333,10 +435,14 @@ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_spee
self.logger = logging.getLogger(__name__)
self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
- # 新增：ACC跟车相关属性
+ # ACC跟车相关属性（V1.0保留）
self.last_lead_speed = 0.0 # 前车上次速度
self.stuck_count = 0 # 卡死计数
- COLLISION_FLAG[self.vehicle_id] = False # 初始化碰撞标志
+
+ # 全局状态初始化（V2.0扩展）
+ global COLLISION_FLAG, OBSTACLE_FLAG
+ COLLISION_FLAG[self.vehicle_id] = False
+ OBSTACLE_FLAG[self.vehicle_id] = False
# 生成车辆
self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
@@ -348,8 +454,8 @@ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_spee
if not self.vehicle:
raise RuntimeError(f"车辆{vehicle_id}生成失败")
- # 创建相机
- self.camera = VehicleCamera(world, self.vehicle, vehicle_id)
+ # 创建传感器（V2.0替换原相机类）
+ self.sensors = VehicleSensors(world, self.vehicle, vehicle_id)
# 初始化控制器
self.pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
@@ -406,13 +512,13 @@ def update(self):
target_wps = current_wp.next(lookahead_dist)
target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
- # 速度控制
+ # 速度控制（基础弯道速度）
curve_speed_factors = [1.0, 0.7, 0.4]
speed_factor = curve_speed_factors[min(curve_level, 2)]
base_target_speed = self.base_speed * speed_factor
base_target_speed = max(8.0, base_target_speed)
- # ========== 新增：精细化ACC跟车+紧急避障逻辑 ==========
+ # ========== V1.0保留：精细化ACC跟车+紧急避障逻辑 ==========
if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
try:
lead_agent = vehicle_agents[self.vehicle_id - 2]
@@ -447,7 +553,19 @@ def update(self):
except Exception as e:
self.logger.warning(f"ACC跟车计算异常：{e}")
- # ========== ACC跟车逻辑结束 ==========
+
+ # ========== V2.0新增：LiDAR障碍物检测与避障 ==========
+ obstacle_dist = self.sensors.get_average_obstacle_distance()
+ OBSTACLE_FLAG[self.vehicle_id] = obstacle_dist < OBSTACLE_WARNING_DIST
+
+ if obstacle_dist < OBSTACLE_EMERGENCY_DIST:
+ # 紧急制动：直接减速至0
+ base_target_speed = max(0.0, current_speed - OBSTACLE_DECEL_RATE)
+ self.logger.warning(f"前方{obstacle_dist:.1f}米检测到障碍物！紧急制动，目标速度：{base_target_speed:.1f}km/h")
+ elif obstacle_dist < OBSTACLE_WARNING_DIST:
+ # 预警减速：降低至基础速度的50%
+ base_target_speed = max(8.0, base_target_speed * 0.5)
+ self.logger.warning(f"前方{obstacle_dist:.1f}米检测到障碍物！预警减速，目标速度：{base_target_speed:.1f}km/h")
# 交通灯处理
target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
@@ -459,15 +577,17 @@ def update(self):
throttle = self.speed_controller.calculate(target_speed, current_speed)
brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
- if "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
- throttle = 0.0
- brake = 1.0
-
- # 新增：碰撞后紧急停车
+ # 状态优先级：碰撞 > 红灯 > 障碍物 > 正常行驶
if COLLISION_FLAG.get(self.vehicle_id, False):
throttle = 0.0
brake = 1.0
self.logger.error("检测到碰撞，紧急停车！")
+ elif "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
+ throttle = 0.0
+ brake = 1.0
+ elif obstacle_dist < OBSTACLE_EMERGENCY_DIST:
+ throttle = 0.0
+ brake = 1.0
# 应用控制
control = carla.VehicleControl()
@@ -476,11 +596,12 @@ def update(self):
control.brake = brake
self.vehicle.apply_control(control)
- # 日志输出（新增ACC相关信息）
+ # 日志输出（新增障碍物信息）
+ obstacle_status = f"障碍物{obstacle_dist:.1f}m" if obstacle_dist < OBSTACLE_WARNING_DIST else "无障碍物"
self.logger.info(
f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | "
f"弯道：{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态：{traffic_light_status} | "
- f"ACC：{'激活' if self.vehicle_id>1 else '未激活'}"
+ f"ACC：{'激活' if self.vehicle_id>1 else '未激活'} | {obstacle_status}"
)
return True
@@ -490,12 +611,12 @@ def update(self):
return False
def destroy(self):
- # 销毁相机
- self.camera.destroy()
+ # 销毁传感器
+ self.sensors.destroy()
# 销毁车辆
if self.vehicle and is_actor_alive(self.vehicle):
self.vehicle.destroy()
- self.logger.info("车辆资源已清理")
+ self.logger.info("车辆及传感器资源已清理")
# ===================== 控制器类 ======================
class AdaptivePurePursuit:
@@ -567,8 +688,7 @@ def calculate(self, target_speed, current_speed):
self.integral = np.clip(self.integral, -1.0, 1.0)
d = self.kd * (error - self.last_error)
self.last_error = error
- # 修复：将 i 改为 self.integral
- return np.clip(p + self.integral + d, 0.0, 1.0)
+ return np.clip(p + self.integral + d, 0.0, 1.0) # 修复原代码i未定义问题
class TrafficLightManager:
def __init__(self, vehicle_id):
@@ -725,7 +845,7 @@ def main():
global current_view_vehicle_id, vehicle_agents
pygame.init()
screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
- pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- V1.0 ACC跟车 - 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
+ pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- V2.0 LiDAR感知 - 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
client = None
world = None
@@ -812,7 +932,7 @@ def cleanup():
print(f"\n生成车辆{i+1}（车型：{vehicle_model}）...")
agent = VehicleAgent(world, map, i+1, spawn_point, vehicle_model, base_speed)
vehicle_agents.append(agent)
- print(f"车辆{i+1}生成成功！")
+ print(f"车辆{i+1}生成成功！已挂载LiDAR+碰撞传感器")
except Exception as e:
print(f"车辆{i+1}生成失败：{e}")
@@ -821,7 +941,7 @@ def cleanup():
if len(vehicle_agents) == 0:
raise RuntimeError("无车辆生成成功，仿真终止")
- print(f"\n共生成{len(vehicle_agents)}辆车辆！V1.0 ACC跟车功能已启用")
+ print(f"\n共生成{len(vehicle_agents)}辆车辆！V2.0 LiDAR感知+障碍物避障功能已启用")
# 创建全局俯视相机
try:
@@ -888,35 +1008,35 @@ def cleanup():
elif show_split_screen:
if len(vehicle_agents) == 1:
agent = vehicle_agents[0]
- if agent.camera.image_surface:
- surface = pygame.transform.scale(agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ if agent.sensors.image_surface:
+ surface = pygame.transform.scale(agent.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
screen.blit(surface, (0, 0))
elif len(vehicle_agents) == 2:
agent1 = vehicle_agents[0]
agent2 = vehicle_agents[1]
- if agent1.camera.image_surface:
- surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ if agent1.sensors.image_surface:
+ surface1 = pygame.transform.scale(agent1.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
screen.blit(surface1, (0, 0))
- if agent2.camera.image_surface:
- surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
+ if agent2.sensors.image_surface:
+ surface2 = pygame.transform.scale(agent2.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
screen.blit(surface2, (WINDOW_WIDTH//2, 0))
elif len(vehicle_agents) >= 3:
agent1 = vehicle_agents[0]
agent2 = vehicle_agents[1]
agent3 = vehicle_agents[2]
- if agent1.camera.image_surface:
- surface1 = pygame.transform.scale(agent1.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ if agent1.sensors.image_surface:
+ surface1 = pygame.transform.scale(agent1.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
screen.blit(surface1, (0, 0))
- if agent2.camera.image_surface:
- surface2 = pygame.transform.scale(agent2.camera.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ if agent2.sensors.image_surface:
+ surface2 = pygame.transform.scale(agent2.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
screen.blit(surface2, (WINDOW_WIDTH//2, 0))
- if agent3.camera.image_surface:
- surface3 = pygame.transform.scale(agent3.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
+ if agent3.sensors.image_surface:
+ surface3 = pygame.transform.scale(agent3.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
screen.blit(surface3, (0, WINDOW_HEIGHT//2))
else:
target_agent = None
@@ -925,8 +1045,8 @@ def cleanup():
target_agent = agent
break
- if target_agent and target_agent.camera.image_surface:
- surface = pygame.transform.scale(target_agent.camera.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ if target_agent and target_agent.sensors.image_surface:
+ surface = pygame.transform.scale(target_agent.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
screen.blit(surface, (0, 0))
# 更新车辆状态
From 0c8e7fedcdfaa1245164fa7e973fd7e4a8c86c2e Mon Sep 17 00:00:00 2001
From: Liyang2302 <2358507952@qq.com>
Date: Wed, 24 Dec 2025 21:37:54 +0800
Subject: [PATCH 26/26] =?UTF-8?q?=E6=8F=90=E5=8D=87=E5=8F=AF=E8=A7=86?=
=?UTF-8?q?=E5=8C=96=E4=B8=8E=E8=B0=83=E8=AF=95=E5=B7=A5=E5=85=B7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/autonomous_driving_car/main.py | 992 +++++++++++++++++------------
1 file changed, 580 insertions(+), 412 deletions(-)
diff --git a/src/autonomous_driving_car/main.py b/src/autonomous_driving_car/main.py
index 45ddb3e1b7..61676adc4d 100644
--- a/src/autonomous_driving_car/main.py
+++ b/src/autonomous_driving_car/main.py
@@ -1,7 +1,13 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
-CARLA 多车辆协同控制版：V2.0 增强感知（LiDAR+碰撞检测+障碍物避障）
+CARLA 多车辆协同控制版：V3.0 终极稳定版
+核心特性：
+1. 彻底修复传感器销毁警告
+2. LiDAR点云处理性能优化（降采样+缓存）
+3. 车辆状态实时监控与故障自动恢复
+4. 精准障碍物避障+ACC跟车+交通灯合规
+5. 多视角流畅切换+性能监控
"""
import sys
@@ -16,8 +22,9 @@
from concurrent.futures import ThreadPoolExecutor, as_completed
import logging
import random
+from collections import deque
-# ===================== 全局配置 =======================
+# ===================== 全局配置（V3.0优化）=======================
# CARLA连接
CARLA_HOST = "localhost"
CARLA_PORT = 2000
@@ -33,9 +40,9 @@
SPAWN_INTERVAL = 1.0
SPAWN_RETRY_MAX = 8
SPAWN_RETRY_DELAY = 0.5
-SPAWN_DISTANCE_LIMIT = 15.0 # 放宽距离限制到15米
+SPAWN_DISTANCE_LIMIT = 15.0
-# 车辆控制参数
+# 车辆控制参数（V3.0优化）
VEHICLE_WHEELBASE = 2.9
VEHICLE_REAR_AXLE_OFFSET = 1.45
LOOKAHEAD_DIST_STRAIGHT = 7.0
@@ -49,24 +56,26 @@
DIR_CHANGE_SHARP = 0.08
BASE_SPEEDS = [25.0, 22.0, 20.0]
PID_KP = 0.2
-PID_KI = 0.01
+PID_KI = 0.008 # 降低积分系数，减少饱和
PID_KD = 0.02
-# ACC跟车配置（V1.0保留）
-SAFE_TIME_GAP = 1.5 # 安全时距（秒）
-MIN_SAFE_DISTANCE = 5.0 # 最小安全距离（米）
-EMERGENCY_DECEL_RATE = 5.0 # 紧急制动减速度（km/h/帧）
-LEAD_BRAKE_THRESHOLD = -10.0 # 前车急刹加速度阈值（km/h/s）
-
-# LiDAR与障碍物检测配置（V2.0新增）
-LIDAR_RANGE = 30.0 # LiDAR检测范围（米）
-LIDAR_POINTS_PER_SECOND = 100000 # LiDAR点云密度
-LIDAR_ROTATION_FREQ = 30 # LiDAR刷新率（Hz）
-OBSTACLE_DETECTION_WIDTH = 2.0 # 检测宽度（左右各2米）
-OBSTACLE_MIN_HEIGHT = 0.5 # 障碍物最小高度（过滤地面）
-OBSTACLE_WARNING_DIST = 8.0 # 障碍物预警距离（米）
-OBSTACLE_EMERGENCY_DIST = 5.0 # 障碍物紧急制动距离（米）
-OBSTACLE_DECEL_RATE = 8.0 # 障碍物制动减速度（km/h/帧）
+# ACC跟车配置
+SAFE_TIME_GAP = 1.5
+MIN_SAFE_DISTANCE = 5.0
+EMERGENCY_DECEL_RATE = 5.0
+LEAD_BRAKE_THRESHOLD = -10.0
+
+# LiDAR与障碍物检测配置（V3.0性能优化）
+LIDAR_RANGE = 30.0
+LIDAR_POINTS_PER_SECOND = 50000 # 降采样，减少计算量
+LIDAR_ROTATION_FREQ = 20 # 降低刷新率，提升性能
+OBSTACLE_DETECTION_WIDTH = 2.0
+OBSTACLE_MIN_HEIGHT = 0.5
+OBSTACLE_MAX_HEIGHT = 3.0 # 新增最大高度过滤，避免误检高空物体
+OBSTACLE_WARNING_DIST = 8.0
+OBSTACLE_EMERGENCY_DIST = 5.0
+OBSTACLE_DECEL_RATE = 8.0
+OBSTACLE_CACHE_SIZE = 5 # 障碍物距离缓存大小，平滑滤波
# 交通规则配置
TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
@@ -85,27 +94,74 @@
CAMERA_FOV = 120
CAMERA_POS = carla.Transform(carla.Location(x=-6.0, z=2.5), carla.Rotation(pitch=-5))
+# 性能监控配置
+PERF_MONITOR_INTERVAL = 1.0 # 性能监控输出间隔（秒）
+VEHICLE_RESTART_THRESHOLD = 5 # 车辆连续故障次数阈值，超过则重启
+
# 全局变量
current_view_vehicle_id = 1
vehicle_agents = []
-COLLISION_FLAG = {} # 碰撞标志（V2.0扩展）
-OBSTACLE_FLAG = {} # 障碍物标志（V2.0新增）
+COLLISION_FLAG = {}
+OBSTACLE_FLAG = {}
+last_perf_time = time.time()
+perf_stats = {"frame_count": 0, "avg_fps": 0.0}
-# 日志配置
+# 日志配置（V3.0增强）
logging.basicConfig(
level=logging.INFO,
format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
- handlers=[logging.FileHandler("multi_vehicle_simulation_v2.log"), logging.StreamHandler()]
+ handlers=[
+ logging.FileHandler("multi_vehicle_simulation_v3.log", encoding="utf-8"),
+ logging.StreamHandler(sys.stdout)
+ ]
)
-# ===================== 核心工具函数 ======================
+# ===================== 核心工具函数（V3.0重构）=====================
def is_actor_alive(actor):
+ """安全检查actor是否存活（终极版）"""
+ if actor is None:
+ return False
try:
return actor.is_alive()
- except TypeError:
- return actor.is_alive
+ except (TypeError, AttributeError):
+ try:
+ return actor.is_alive
+ except:
+ return False
+
+def is_sensor_listening(sensor):
+ """检查传感器是否在监听数据"""
+ if sensor is None or not is_actor_alive(sensor):
+ return False
+ try:
+ sensor.listen(lambda data: None)
+ return True
+ except:
+ return False
+
+def safe_sensor_stop(sensor, sensor_name, logger):
+ """安全停止传感器监听"""
+ if sensor is None or not is_actor_alive(sensor):
+ return
+ try:
+ if is_sensor_listening(sensor):
+ sensor.stop()
+ logger.debug(f"{sensor_name}监听已停止")
+ except Exception as e:
+ logger.warning(f"停止{sensor_name}监听忽略异常：{str(e)[:50]}")
+
+def safe_actor_destroy(actor, actor_name, logger):
+ """安全销毁actor"""
+ if actor is None or not is_actor_alive(actor):
+ return
+ try:
+ actor.destroy()
+ logger.debug(f"{actor_name}已销毁")
+ except Exception as e:
+ logger.warning(f"销毁{actor_name}忽略异常：{str(e)[:50]}")
def get_traffic_light_stop_line(traffic_light):
+ """获取交通灯停止线位置（容错版）"""
try:
return traffic_light.get_stop_line_location()
except AttributeError:
@@ -116,6 +172,7 @@ def get_traffic_light_stop_line(traffic_light):
return stop_line_loc
def calculate_dir_change(current_wp):
+ """计算道路方向变化（优化版）"""
waypoints = [current_wp]
for i in range(5):
next_wps = waypoints[-1].next(1.0)
@@ -141,32 +198,46 @@ def calculate_dir_change(current_wp):
for i in range(1, len(dirs)):
dir_change += abs(dirs[i] - dirs[i-1]) * 2
- if dir_change < DIR_CHANGE_GENTLE:
- curve_level = 0
- elif dir_change < DIR_CHANGE_SHARP:
+ curve_level = 0
+ if dir_change >= DIR_CHANGE_GENTLE:
curve_level = 1
- else:
+ if dir_change >= DIR_CHANGE_SHARP:
curve_level = 2
return dir_change, curve_level
-def get_forward_waypoint(vehicle, map):
+def get_forward_waypoint(vehicle, map, wp_cache=None):
+ """获取前进方向路点（带缓存优化）"""
vehicle_transform = vehicle.get_transform()
+ cache_key = (round(vehicle_transform.location.x, 1), round(vehicle_transform.location.y, 1))
+
+ # 缓存命中直接返回
+ if wp_cache and cache_key in wp_cache:
+ return wp_cache[cache_key]
+
current_wp = map.get_waypoint(
vehicle_transform.location,
project_to_road=True,
lane_type=carla.LaneType.Driving
)
- # 所有车辆使用同一车道的路点
+ # 所有车辆使用同一车道
global vehicle_agents
if len(vehicle_agents) > 0:
try:
- lead_vehicle_wp = map.get_waypoint(vehicle_agents[0].vehicle.get_transform().location, project_to_road=True)
- current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True, lane_id=lead_vehicle_wp.lane_id)
+ lead_vehicle_wp = map.get_waypoint(
+ vehicle_agents[0].vehicle.get_transform().location,
+ project_to_road=True
+ )
+ current_wp = map.get_waypoint(
+ vehicle_transform.location,
+ project_to_road=True,
+ lane_id=lead_vehicle_wp.lane_id
+ )
except:
pass
+ # 方向检查与修正
road_direction = current_wp.transform.get_forward_vector()
vehicle_direction = vehicle_transform.get_forward_vector()
dot_product = road_direction.x * vehicle_direction.x + road_direction.y * vehicle_direction.y
@@ -180,92 +251,52 @@ def get_forward_waypoint(vehicle, map):
vehicle_transform.location + vehicle_direction * 5.0,
project_to_road=True
)
-
+
+ # 更新缓存（有效期短，避免过时）
+ if wp_cache:
+ wp_cache[cache_key] = current_wp
+ # 缓存清理：只保留最近100个
+ if len(wp_cache) > 100:
+ wp_cache.pop(next(iter(wp_cache)))
+
return current_wp
def get_valid_spawn_points(map, count, base_location=None, radius=100.0):
- """
- 获取有效的出生点（增加容错性，避免索引越界）
- """
- # 1. 获取地图所有出生点
+ """获取有效出生点（终极容错版）"""
all_spawn_points = map.get_spawn_points()
if not all_spawn_points:
raise RuntimeError("地图中无任何出生点")
- # 2. 初始化候选点列表
candidate_points = []
-
- # 3. 如果有基准位置，先筛选附近的点；否则直接使用所有点
if base_location:
- filtered_points = []
- for sp in all_spawn_points:
- dist = sp.location.distance(base_location)
- if dist <= radius:
- filtered_points.append((dist, sp))
- # 按距离排序
- filtered_points.sort(key=lambda x: x[0])
- candidate_points = [sp for _, sp in filtered_points]
-
- # 4. 如果候选点为空，直接使用所有出生点（容错）
- if not candidate_points:
+ filtered_points = [(sp.location.distance(base_location), sp) for sp in all_spawn_points]
+ filtered_points = [sp for dist, sp in sorted(filtered_points) if dist <= radius]
+ candidate_points = filtered_points if filtered_points else all_spawn_points
+ else:
candidate_points = all_spawn_points
- print(f"警告：基准位置{base_location}附近无出生点，使用全局出生点")
- # 5. 筛选集中的出生点（放宽条件）
+ # 筛选集中的出生点
valid_points = []
- # 确保基准点存在（核心修复：避免candidate_points[0]索引越界）
- if not candidate_points:
- candidate_points = all_spawn_points
+ if candidate_points:
+ base_sp = candidate_points[0]
+ valid_points.append(base_sp)
- base_sp = candidate_points[0]
- valid_points.append(base_sp)
-
- # 6. 筛选其他点，放宽距离限制
- for sp in candidate_points[1:]:
- try:
- # 检查与已选点的距离（放宽到15米）
- if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT for vp in valid_points):
- wp = map.get_waypoint(sp.location, project_to_road=True)
- if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
- valid_points.append(sp)
- if len(valid_points) >= count:
- break
- except:
- continue
-
- # 7. 如果数量不够，进一步放宽条件（距离限制到20米）
- if len(valid_points) < count:
- for sp in candidate_points:
- if sp not in valid_points:
- try:
- if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT * 1.5 for vp in valid_points):
- wp = map.get_waypoint(sp.location, project_to_road=True)
- if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
- valid_points.append(sp)
- if len(valid_points) >= count:
- break
- except:
- continue
-
- # 8. 如果还是不够，直接取前N个点（最终容错）
- if len(valid_points) < count:
- print(f"警告：无法找到{count}个集中的出生点，直接取前{count}个可用点")
- for sp in candidate_points:
- if sp not in valid_points:
- wp = map.get_waypoint(sp.location, project_to_road=True)
- if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
- valid_points.append(sp)
+ for sp in candidate_points[1:]:
if len(valid_points) >= count:
break
+ try:
+ if all(sp.location.distance(vp.location) <= SPAWN_DISTANCE_LIMIT for vp in valid_points):
+ wp = map.get_waypoint(sp.location, project_to_road=True)
+ if wp.lane_type == carla.LaneType.Driving and 0.0 <= sp.location.z <= 2.0:
+ valid_points.append(sp)
+ except:
+ continue
- # 9. 最终检查：确保数量足够
- if len(valid_points) < count:
- # 直接取所有可用点，不足的话重复使用（极端情况）
- while len(valid_points) < count:
- valid_points.append(valid_points[0])
- print(f"警告：出生点数量不足，重复使用已有点")
+ # 最终容错
+ while len(valid_points) < count:
+ valid_points.append(valid_points[0] if valid_points else all_spawn_points[0])
- # 10. 统一出生点朝向
+ # 统一朝向
try:
forward_vec = valid_points[0].transform.get_forward_vector()
for sp in valid_points:
@@ -276,303 +307,376 @@ def get_valid_spawn_points(map, count, base_location=None, radius=100.0):
return valid_points[:count]
def check_spawn_collision(world, spawn_point, radius=3.0):
- # 检查周围车辆和行人
- vehicles = world.get_actors().filter("vehicle.*")
- for vehicle in vehicles:
- if is_actor_alive(vehicle):
- dist = vehicle.get_transform().location.distance(spawn_point.location)
- if dist < radius:
- return False
-
- walkers = world.get_actors().filter("walker.*")
- for walker in walkers:
- if is_actor_alive(walker):
- dist = walker.get_transform().location.distance(spawn_point.location)
- if dist < radius:
- return False
-
+ """检查出生点碰撞"""
+ for vehicle in world.get_actors().filter("vehicle.*"):
+ if is_actor_alive(vehicle) and vehicle.get_transform().location.distance(spawn_point.location) < radius:
+ return False
+ for walker in world.get_actors().filter("walker.*"):
+ if is_actor_alive(walker) and walker.get_transform().location.distance(spawn_point.location) < radius:
+ return False
return True
-# ===================== 传感器管理类（V2.0重构：相机+LiDAR+碰撞）=====================
+def print_performance_stats():
+ """打印性能统计信息"""
+ global perf_stats, last_perf_time
+ current_time = time.time()
+ if current_time - last_perf_time < PERF_MONITOR_INTERVAL:
+ return
+
+ # 计算FPS
+ elapsed = current_time - last_perf_time
+ fps = perf_stats["frame_count"] / elapsed if elapsed > 0 else 0
+ perf_stats["avg_fps"] = (perf_stats["avg_fps"] * 0.9) + (fps * 0.1) # 指数平滑
+
+ # 车辆状态统计
+ alive_vehicles = sum(1 for agent in vehicle_agents if agent.is_alive)
+ collision_count = sum(1 for v_id in COLLISION_FLAG if COLLISION_FLAG[v_id])
+ obstacle_count = sum(1 for v_id in OBSTACLE_FLAG if OBSTACLE_FLAG[v_id])
+
+ # 控制台输出
+ print(f"\n=== 性能监控 [{time.strftime('%H:%M:%S')}] ===")
+ print(f"平均FPS: {perf_stats['avg_fps']:.1f} | 活跃车辆: {alive_vehicles}/{VEHICLE_COUNT}")
+ print(f"碰撞车辆数: {collision_count} | 障碍物预警数: {obstacle_count}")
+ print("="*50)
+
+ # 重置统计
+ perf_stats["frame_count"] = 0
+ last_perf_time = current_time
+
+# ===================== 传感器管理类（V3.0终极版）=====================
class VehicleSensors:
def __init__(self, world, vehicle, vehicle_id):
self.world = world
self.vehicle = vehicle
self.vehicle_id = vehicle_id
- self.logger = logging.getLogger(__name__)
- self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
-
+ self.logger = logging.LoggerAdapter(logging.getLogger(__name__), {"vehicle_id": vehicle_id})
+
+ # 状态标记
+ self.is_destroyed = False
+ self.init_success = False
+
# 传感器实例
self.camera = None
self.lidar = None
self.collision_sensor = None
-
- # 数据存储
+
+ # 数据存储（V3.0优化）
self.image_surface = None
- self.obstacle_distances = [] # 前方障碍物距离列表
- self.last_obstacle_dist = float('inf') # 最近障碍物距离
-
- # 创建所有传感器
- self._create_camera()
- self._create_lidar()
- self._create_collision_sensor()
+ self.obstacle_dist_cache = deque(maxlen=OBSTACLE_CACHE_SIZE) # 环形缓存
+ self.last_obstacle_dist = float('inf')
+
+ # 初始化传感器
+ try:
+ self._create_camera()
+ self._create_lidar()
+ self._create_collision_sensor()
+ self.init_success = True
+ self.logger.info("所有传感器初始化成功")
+ except Exception as e:
+ self.logger.error(f"传感器初始化失败：{str(e)[:100]}")
+ self.destroy()
def _create_camera(self):
- """创建RGB相机传感器"""
+ """创建RGB相机"""
camera_bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
- camera_bp.set_attribute("image_size_x", str(640))
- camera_bp.set_attribute("image_size_y", str(360))
+ camera_bp.set_attribute("image_size_x", "640")
+ camera_bp.set_attribute("image_size_y", "360")
camera_bp.set_attribute("fov", str(CAMERA_FOV))
-
- # 生成相机（附加到车辆）
+ camera_bp.set_attribute("sensor_tick", "0.033") # 30Hz
+
self.camera = self.world.spawn_actor(camera_bp, CAMERA_POS, attach_to=self.vehicle)
- # 注册图像回调函数
self.camera.listen(self._on_image)
def _create_lidar(self):
- """创建LiDAR传感器"""
+ """创建LiDAR传感器（V3.0性能优化）"""
lidar_bp = self.world.get_blueprint_library().find("sensor.lidar.ray_cast")
- # 设置LiDAR参数
+ # 性能优化参数
lidar_bp.set_attribute("range", str(LIDAR_RANGE))
lidar_bp.set_attribute("points_per_second", str(LIDAR_POINTS_PER_SECOND))
lidar_bp.set_attribute("rotation_frequency", str(LIDAR_ROTATION_FREQ))
- lidar_bp.set_attribute("channels", "32") # 32线LiDAR
- lidar_bp.set_attribute("upper_fov", "15")
- lidar_bp.set_attribute("lower_fov", "-25")
- lidar_bp.set_attribute("points_per_second", str(LIDAR_POINTS_PER_SECOND))
-
- # LiDAR挂载位置（车顶）
+ lidar_bp.set_attribute("channels", "16") # 16线代替32线，降低计算量
+ lidar_bp.set_attribute("upper_fov", "10")
+ lidar_bp.set_attribute("lower_fov", "-20")
+ lidar_bp.set_attribute("sensor_tick", str(1.0/LIDAR_ROTATION_FREQ))
+
lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.0))
self.lidar = self.world.spawn_actor(lidar_bp, lidar_transform, attach_to=self.vehicle)
- # 注册LiDAR回调函数
self.lidar.listen(self._on_lidar_data)
def _create_collision_sensor(self):
"""创建碰撞传感器"""
collision_bp = self.world.get_blueprint_library().find("sensor.other.collision")
self.collision_sensor = self.world.spawn_actor(collision_bp, carla.Transform(), attach_to=self.vehicle)
- # 注册碰撞回调函数
self.collision_sensor.listen(self._on_collision)
def _on_image(self, image):
- """相机图像回调：转换为Pygame Surface"""
- array = np.frombuffer(image.raw_data, dtype=np.uint8)
- array = array.reshape((image.height, image.width, 4))
- array = array[:, :, :3]
- array = array[:, :, ::-1]
- array = np.swapaxes(array, 0, 1)
- self.image_surface = pygame.surfarray.make_surface(array)
+ """相机图像回调（非阻塞）"""
+ if self.is_destroyed:
+ return
+ try:
+ array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))
+ array = array[:, :, :3][:, :, ::-1] # BGR转RGB
+ array = np.swapaxes(array, 0, 1)
+ self.image_surface = pygame.surfarray.make_surface(array)
+ except Exception as e:
+ self.logger.error(f"图像处理失败：{str(e)[:50]}")
def _on_lidar_data(self, data):
- """LiDAR点云回调：解析并提取前方障碍物"""
+ """LiDAR点云回调（V3.0优化）"""
+ if self.is_destroyed:
+ return
try:
- # 将点云数据转换为numpy数组 (x, y, z, intensity)
- points = np.frombuffer(data.raw_data, dtype=np.float32).reshape(-1, 4)
+ # 点云降采样（每N个点取1个）
+ points = np.frombuffer(data.raw_data, dtype=np.float32).reshape(-1, 4)[::2] # 降采样50%
- # 过滤条件：
- # 1. 前方（x>0）
- # 2. 左右范围内（|y| < 检测宽度）
- # 3. 非地面（z > 最小高度）
+ # 精准过滤障碍物
front_obstacle_points = points[
- (points[:, 0] > 0) &
- (np.abs(points[:, 1]) < OBSTACLE_DETECTION_WIDTH) &
- (points[:, 2] > OBSTACLE_MIN_HEIGHT)
+ (points[:, 0] > 0) & # 前方
+ (np.abs(points[:, 1]) < OBSTACLE_DETECTION_WIDTH) & # 左右范围
+ (points[:, 2] > OBSTACLE_MIN_HEIGHT) & # 最小高度
+ (points[:, 2] < OBSTACLE_MAX_HEIGHT) # 最大高度
]
- # 计算最近障碍物距离
+ # 更新障碍物距离
if len(front_obstacle_points) > 0:
self.last_obstacle_dist = np.min(front_obstacle_points[:, 0])
- self.obstacle_distances.append(self.last_obstacle_dist)
- # 只保留最近10帧数据（平滑滤波）
- if len(self.obstacle_distances) > 10:
- self.obstacle_distances.pop(0)
+ self.obstacle_dist_cache.append(self.last_obstacle_dist)
else:
self.last_obstacle_dist = float('inf')
- self.obstacle_distances.clear()
+ if self.obstacle_dist_cache:
+ self.obstacle_dist_cache.popleft()
except Exception as e:
- self.logger.error(f"LiDAR数据解析失败：{e}")
+ self.logger.error(f"LiDAR处理失败：{str(e)[:50]}")
def _on_collision(self, event):
- """碰撞回调：记录碰撞信息并触发紧急停车"""
+ """碰撞回调"""
+ if self.is_destroyed:
+ return
try:
- collision_actor_type = event.other_actor.type_id
- collision_location = event.transform.location
+ collision_actor = event.other_actor
+ collision_type = collision_actor.type_id if collision_actor else "未知"
+ collision_loc = event.transform.location
self.logger.error(
- f"发生碰撞！碰撞对象：{collision_actor_type} | 碰撞位置：({collision_location.x:.1f}, {collision_location.y:.1f})"
+ f"碰撞发生！对象：{collision_type} | 位置：({collision_loc.x:.1f}, {collision_loc.y:.1f})"
)
- # 设置碰撞标志
global COLLISION_FLAG
COLLISION_FLAG[self.vehicle_id] = True
except Exception as e:
- self.logger.error(f"碰撞检测回调失败：{e}")
+ self.logger.error(f"碰撞检测失败：{str(e)[:50]}")
- def get_average_obstacle_distance(self):
+ def get_smooth_obstacle_distance(self):
"""获取平滑后的障碍物距离"""
- if len(self.obstacle_distances) == 0:
+ if not self.obstacle_dist_cache:
return float('inf')
- return np.mean(self.obstacle_distances)
+ return np.mean(self.obstacle_dist_cache)
def destroy(self):
- """销毁所有传感器"""
- sensors = [self.camera, self.lidar, self.collision_sensor]
- for sensor in sensors:
- if sensor:
- try:
- sensor.stop()
- sensor.destroy()
- except:
- pass
- self.logger.info("所有传感器已销毁")
+ """安全销毁传感器（终极版）"""
+ if self.is_destroyed:
+ return
+
+ self.is_destroyed = True
+ self.logger.info("开始销毁传感器")
+
+ # 停止监听
+ safe_sensor_stop(self.camera, "RGB相机", self.logger)
+ safe_sensor_stop(self.lidar, "LiDAR", self.logger)
+ safe_sensor_stop(self.collision_sensor, "碰撞传感器", self.logger)
+
+ # 销毁传感器
+ safe_actor_destroy(self.camera, "RGB相机", self.logger)
+ safe_actor_destroy(self.lidar, "LiDAR", self.logger)
+ safe_actor_destroy(self.collision_sensor, "碰撞传感器", self.logger)
+
+ # 清空引用
+ self.camera = None
+ self.lidar = None
+ self.collision_sensor = None
+ self.image_surface = None
+
+ self.logger.info("传感器销毁完成")
-# ===================== 车辆控制类 ======================
+# ===================== 车辆控制类（V3.0终极版）=====================
class VehicleAgent:
def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_speed):
self.vehicle_id = vehicle_id
self.world = world
self.map = map
self.base_speed = base_speed
- self.logger = logging.getLogger(__name__)
- self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
-
- # ACC跟车相关属性（V1.0保留）
- self.last_lead_speed = 0.0 # 前车上次速度
- self.stuck_count = 0 # 卡死计数
+ self.logger = logging.LoggerAdapter(logging.getLogger(__name__), {"vehicle_id": vehicle_id})
+
+ # 状态管理
+ self.is_alive = True
+ self.fault_count = 0 # 故障计数
+ self.wp_cache = {} # 路点缓存
+ self.last_update_success = True
+
+ # ACC跟车属性
+ self.last_lead_speed = 0.0
+ self.last_lead_acc = 0.0
- # 全局状态初始化（V2.0扩展）
+ # 全局状态初始化
global COLLISION_FLAG, OBSTACLE_FLAG
COLLISION_FLAG[self.vehicle_id] = False
OBSTACLE_FLAG[self.vehicle_id] = False
# 生成车辆
- self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
- if self.vehicle_bp.has_attribute("color"):
- color = random.choice(self.vehicle_bp.get_attribute("color").recommended_values)
- self.vehicle_bp.set_attribute("color", color)
-
- self.vehicle = self._spawn_vehicle_with_retry(spawn_point)
- if not self.vehicle:
- raise RuntimeError(f"车辆{vehicle_id}生成失败")
-
- # 创建传感器（V2.0替换原相机类）
- self.sensors = VehicleSensors(world, self.vehicle, vehicle_id)
-
- # 初始化控制器
- self.pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
- self.speed_controller = SpeedController(PID_KP, PID_KI, PID_KD, base_speed)
- self.traffic_light_manager = TrafficLightManager(vehicle_id)
-
- self.is_alive = True
- self.logger.info(f"生成成功，车型：{vehicle_model}，出生点：({spawn_point.location.x:.1f},{spawn_point.location.y:.1f})")
-
- def _spawn_vehicle_with_retry(self, initial_spawn_point):
- all_spawn_points = self.map.get_spawn_points()
- if not all_spawn_points:
- self.logger.error("地图中无有效出生点")
- return None
+ self.vehicle = None
+ self.sensors = None
+ try:
+ self._spawn_vehicle(spawn_point, vehicle_model)
+ self._init_sensors()
+ self._init_controllers()
+ self.logger.info(f"车辆生成成功 | 车型：{vehicle_model} | 出生点：({spawn_point.location.x:.1f},{spawn_point.location.y:.1f})")
+ except Exception as e:
+ self.logger.error(f"车辆初始化失败：{str(e)[:100]}")
+ self.is_alive = False
- candidate_points = [initial_spawn_point]
- candidate_points += random.sample(all_spawn_points, min(10, len(all_spawn_points)))
+ def _spawn_vehicle(self, spawn_point, vehicle_model):
+ """生成车辆（带重试）"""
+ vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
+ if vehicle_bp.has_attribute("color"):
+ color = random.choice(vehicle_bp.get_attribute("color").recommended_values)
+ vehicle_bp.set_attribute("color", color)
+ candidate_points = [spawn_point] + random.sample(self.map.get_spawn_points(), min(5, len(self.map.get_spawn_points())))
+
for retry in range(SPAWN_RETRY_MAX):
- spawn_point = candidate_points[retry % len(candidate_points)]
- spawn_point.location.z += 0.3
- spawn_point.rotation.yaw += random.randint(-5, 5)
-
- if not check_spawn_collision(self.world, spawn_point):
- self.logger.warning(f"第{retry+1}次重试：出生点有碰撞风险，跳过")
+ current_sp = candidate_points[retry % len(candidate_points)]
+ current_sp.location.z += 0.3
+ current_sp.rotation.yaw += random.randint(-5, 5)
+
+ if not check_spawn_collision(self.world, current_sp):
+ self.logger.warning(f"出生点碰撞风险，重试{retry+1}/{SPAWN_RETRY_MAX}")
time.sleep(SPAWN_RETRY_DELAY)
continue
-
+
try:
- return self.world.spawn_actor(self.vehicle_bp, spawn_point)
+ self.vehicle = self.world.spawn_actor(vehicle_bp, current_sp)
+ self.logger.debug(f"车辆生成重试{retry+1}成功")
+ return
except Exception as e:
- self.logger.warning(f"第{retry+1}次重试失败：{e}")
+ self.logger.warning(f"车辆生成重试{retry+1}失败：{str(e)[:50]}")
time.sleep(SPAWN_RETRY_DELAY)
+
+ raise RuntimeError(f"超过{SPAWN_RETRY_MAX}次重试，车辆生成失败")
+
+ def _init_sensors(self):
+ """初始化传感器"""
+ self.sensors = VehicleSensors(self.world, self.vehicle, self.vehicle_id)
+ if not self.sensors.init_success:
+ raise RuntimeError("传感器初始化失败")
- self.logger.error(f"超过{SPAWN_RETRY_MAX}次重试，生成失败")
- return None
+ def _init_controllers(self):
+ """初始化控制器"""
+ self.pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
+ self.speed_controller = SpeedController(PID_KP, PID_KI, PID_KD, self.base_speed)
+ self.traffic_light_manager = TrafficLightManager(self.vehicle_id)
+
+ def _restart_vehicle(self):
+ """重启故障车辆"""
+ self.logger.warning(f"车辆故障次数达到阈值，尝试重启")
+
+ # 销毁旧车辆
+ self.destroy()
+
+ # 重新生成
+ try:
+ spawn_points = get_valid_spawn_points(self.map, 1, self.vehicle.get_transform().location if self.vehicle else None)
+ self._spawn_vehicle(spawn_points[0], VEHICLE_MODELS[self.vehicle_id % len(VEHICLE_MODELS)])
+ self._init_sensors()
+ self._init_controllers()
+
+ # 重置状态
+ self.is_alive = True
+ self.fault_count = 0
+ self.last_update_success = True
+ COLLISION_FLAG[self.vehicle_id] = False
+ OBSTACLE_FLAG[self.vehicle_id] = False
+
+ self.logger.info("车辆重启成功")
+ except Exception as e:
+ self.logger.error(f"车辆重启失败：{str(e)[:100]}")
+ self.is_alive = False
def update(self):
+ """更新车辆状态（V3.0增强）"""
if not self.is_alive or not is_actor_alive(self.vehicle):
self.is_alive = False
self.logger.error("车辆已销毁，停止更新")
return False
try:
- # 获取车辆状态
+ # 获取车辆基础状态
vehicle_transform = self.vehicle.get_transform()
vehicle_vel = self.vehicle.get_velocity()
current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6
# 路径跟踪
- current_wp = get_forward_waypoint(self.vehicle, self.map)
+ current_wp = get_forward_waypoint(self.vehicle, self.map, self.wp_cache)
dir_change, curve_level = calculate_dir_change(current_wp)
lookahead_dist = self.pp_controller.get_adaptive_lookahead(dir_change)
+
target_wps = current_wp.next(lookahead_dist)
target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location
- # 速度控制（基础弯道速度）
+ # 基础速度计算（弯道减速）
curve_speed_factors = [1.0, 0.7, 0.4]
speed_factor = curve_speed_factors[min(curve_level, 2)]
- base_target_speed = self.base_speed * speed_factor
- base_target_speed = max(8.0, base_target_speed)
+ base_target_speed = max(8.0, self.base_speed * speed_factor)
- # ========== V1.0保留：精细化ACC跟车+紧急避障逻辑 ==========
+ # ========== ACC跟车逻辑（V3.0优化）==========
if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
try:
lead_agent = vehicle_agents[self.vehicle_id - 2]
- lead_vehicle = lead_agent.vehicle
- lead_vehicle_transform = lead_vehicle.get_transform()
-
- # 计算前车速度和加速度
- lead_vel = lead_vehicle.get_velocity()
- lead_speed = math.hypot(lead_vel.x, lead_vel.y) * 3.6
- lead_acc = (lead_speed - lead_agent.last_lead_speed) / 0.03 # 30Hz刷新率，计算加速度
- lead_agent.last_lead_speed = lead_speed # 更新前车上次速度
-
- # 计算安全跟车距离（安全时距+最小安全距）
- safe_dist = (current_speed / 3.6) * SAFE_TIME_GAP + MIN_SAFE_DISTANCE
- dist_to_lead = vehicle_transform.location.distance(lead_vehicle_transform.location)
-
- # 动态调整目标速度
- if dist_to_lead < safe_dist - 2:
- # 过近：减速至前车速度-2（不低于5km/h）
- base_target_speed = max(5.0, lead_speed - 2)
- elif dist_to_lead > safe_dist + 2:
- # 过远：加速至前车速度+2（不超基础速度）
- base_target_speed = min(self.base_speed * speed_factor, lead_speed + 2)
- else:
- # 安全距离：与前车速度同步
- base_target_speed = lead_speed
-
- # 紧急避障：前车急刹（加速度<阈值）
- if lead_acc < LEAD_BRAKE_THRESHOLD:
- base_target_speed = max(0.0, current_speed - EMERGENCY_DECEL_RATE)
- self.logger.warning(f"前车急刹（加速度{lead_acc:.1f}km/h/s）！紧急减速至{base_target_speed:.1f}km/h")
+ if lead_agent.is_alive and is_actor_alive(lead_agent.vehicle):
+ lead_vehicle = lead_agent.vehicle
+ lead_transform = lead_vehicle.get_transform()
+ lead_vel = lead_vehicle.get_velocity()
+ lead_speed = math.hypot(lead_vel.x, lead_vel.y) * 3.6
+
+ # 加速度平滑
+ lead_acc = (lead_speed - lead_agent.last_lead_speed) / (1.0/30)
+ self.last_lead_acc = 0.8 * self.last_lead_acc + 0.2 * lead_acc
+ lead_agent.last_lead_speed = lead_speed
+ # 安全距离计算
+ safe_dist = (current_speed / 3.6) * SAFE_TIME_GAP + MIN_SAFE_DISTANCE
+ dist_to_lead = vehicle_transform.location.distance(lead_transform.location)
+
+ # 动态速度调整
+ if dist_to_lead < safe_dist - 2:
+ base_target_speed = max(5.0, lead_speed - 2)
+ elif dist_to_lead > safe_dist + 2:
+ base_target_speed = min(self.base_speed * speed_factor, lead_speed + 2)
+ else:
+ base_target_speed = lead_speed
+
+ # 前车急刹检测
+ if self.last_lead_acc < LEAD_BRAKE_THRESHOLD:
+ base_target_speed = max(0.0, current_speed - EMERGENCY_DECEL_RATE)
+ self.logger.warning(f"前车急刹！加速度{self.last_lead_acc:.1f}km/h/s，紧急减速")
except Exception as e:
- self.logger.warning(f"ACC跟车计算异常：{e}")
+ self.logger.warning(f"ACC跟车异常：{str(e)[:50]}")
- # ========== V2.0新增：LiDAR障碍物检测与避障 ==========
- obstacle_dist = self.sensors.get_average_obstacle_distance()
+ # ========== 障碍物避障逻辑（V3.0优化）==========
+ obstacle_dist = self.sensors.get_smooth_obstacle_distance()
OBSTACLE_FLAG[self.vehicle_id] = obstacle_dist < OBSTACLE_WARNING_DIST
if obstacle_dist < OBSTACLE_EMERGENCY_DIST:
- # 紧急制动：直接减速至0
base_target_speed = max(0.0, current_speed - OBSTACLE_DECEL_RATE)
- self.logger.warning(f"前方{obstacle_dist:.1f}米检测到障碍物！紧急制动，目标速度：{base_target_speed:.1f}km/h")
+ self.logger.warning(f"前方{obstacle_dist:.1f}米障碍物！紧急制动")
elif obstacle_dist < OBSTACLE_WARNING_DIST:
- # 预警减速：降低至基础速度的50%
base_target_speed = max(8.0, base_target_speed * 0.5)
- self.logger.warning(f"前方{obstacle_dist:.1f}米检测到障碍物！预警减速，目标速度：{base_target_speed:.1f}km/h")
+ self.logger.warning(f"前方{obstacle_dist:.1f}米障碍物！预警减速")
- # 交通灯处理
+ # ========== 交通灯处理 ==========
target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
self.vehicle, current_speed, base_target_speed
)
- # 计算控制指令
+ # ========== 控制指令计算 ==========
steer = self.pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)
throttle = self.speed_controller.calculate(target_speed, current_speed)
brake = 1.0 - throttle if current_speed > target_speed + 1 else 0.0
@@ -581,8 +685,8 @@ def update(self):
if COLLISION_FLAG.get(self.vehicle_id, False):
throttle = 0.0
brake = 1.0
- self.logger.error("检测到碰撞，紧急停车！")
- elif "Red (Stopped)" in traffic_light_status or target_speed == 0.0:
+ self.logger.error("碰撞触发紧急停车")
+ elif "Red (Stopped)" in traffic_light_status or target_speed <= STOP_SPEED_THRESHOLD:
throttle = 0.0
brake = 1.0
elif obstacle_dist < OBSTACLE_EMERGENCY_DIST:
@@ -596,7 +700,7 @@ def update(self):
control.brake = brake
self.vehicle.apply_control(control)
- # 日志输出（新增障碍物信息）
+ # 日志输出
obstacle_status = f"障碍物{obstacle_dist:.1f}m" if obstacle_dist < OBSTACLE_WARNING_DIST else "无障碍物"
self.logger.info(
f"速度：{current_speed:5.1f}km/h | 目标：{target_speed:5.1f} | "
@@ -604,28 +708,50 @@ def update(self):
f"ACC：{'激活' if self.vehicle_id>1 else '未激活'} | {obstacle_status}"
)
+ self.last_update_success = True
+ self.fault_count = 0 # 重置故障计数
return True
except Exception as e:
- self.logger.error(f"更新失败：{e}", exc_info=True)
+ self.logger.error(f"更新失败：{str(e)[:100]}", exc_info=False)
+ self.last_update_success = False
+ self.fault_count += 1
+
+ # 故障重启逻辑
+ if self.fault_count >= VEHICLE_RESTART_THRESHOLD:
+ self._restart_vehicle()
+
return False
def destroy(self):
+ """安全销毁车辆（V3.0终极版）"""
+ self.logger.info("开始销毁车辆资源")
+
# 销毁传感器
- self.sensors.destroy()
+ if self.sensors:
+ self.sensors.destroy()
+
# 销毁车辆
- if self.vehicle and is_actor_alive(self.vehicle):
- self.vehicle.destroy()
- self.logger.info("车辆及传感器资源已清理")
+ safe_actor_destroy(self.vehicle, f"车辆{self.vehicle_id}", self.logger)
+
+ # 清空状态
+ self.vehicle = None
+ self.sensors = None
+ self.is_alive = False
+ self.wp_cache.clear()
+
+ self.logger.info("车辆资源销毁完成")
-# ===================== 控制器类 ======================
+# ===================== 控制器类（V3.0优化）=====================
class AdaptivePurePursuit:
+ """自适应纯追踪控制器"""
def __init__(self, wheelbase):
self.wheelbase = wheelbase
self.last_steer = 0.0
self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT
def calculate_steer(self, vehicle_transform, target_point, dir_change):
+ """计算转向角（优化滤波）"""
forward_vec = vehicle_transform.get_forward_vector()
rear_axle_loc = carla.Location(
x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
@@ -640,6 +766,7 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)
+ # 转向增益自适应
steer_gain = np.interp(
dir_change,
[0, DIR_CHANGE_SHARP],
@@ -647,6 +774,7 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
)
steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)
+ # 计算转向角
if dx_vehicle < 0.1:
steer = self.last_steer
else:
@@ -654,6 +782,7 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
steer = steer_rad / math.pi
steer *= steer_gain
+ # 死区和低通滤波
if abs(steer) < STEER_DEADZONE:
steer = 0.0
steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer
@@ -663,6 +792,7 @@ def calculate_steer(self, vehicle_transform, target_point, dir_change):
return steer
def get_adaptive_lookahead(self, dir_change):
+ """自适应前瞻距离"""
lookahead_dist = np.interp(
dir_change,
[0, DIR_CHANGE_SHARP],
@@ -673,6 +803,7 @@ def get_adaptive_lookahead(self, dir_change):
return lookahead_dist
class SpeedController:
+ """PID速度控制器（V3.0优化）"""
def __init__(self, kp, ki, kd, base_speed):
self.kp = kp
self.ki = ki
@@ -680,26 +811,37 @@ def __init__(self, kp, ki, kd, base_speed):
self.base_speed = base_speed
self.last_error = 0.0
self.integral = 0.0
+ self.integral_limit = 0.5 # 积分限幅
def calculate(self, target_speed, current_speed):
+ """计算油门"""
error = target_speed - current_speed
+
+ # PID计算
p = self.kp * error
self.integral += self.ki * error
- self.integral = np.clip(self.integral, -1.0, 1.0)
+ self.integral = np.clip(self.integral, -self.integral_limit, self.integral_limit) # 积分限幅
d = self.kd * (error - self.last_error)
+
+ # 输出限幅
+ output = np.clip(p + self.integral + d, 0.0, 1.0)
+
self.last_error = error
- return np.clip(p + self.integral + d, 0.0, 1.0) # 修复原代码i未定义问题
+ return output
class TrafficLightManager:
+ """交通灯管理器（V3.0缓存优化）"""
def __init__(self, vehicle_id):
self.vehicle_id = vehicle_id
self.tracked_light = None
self.is_stopped_at_red = False
self.red_light_stop_time = 0
- self.logger = logging.getLogger(__name__)
- self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})
+ self.tl_state_cache = {} # 交通灯状态缓存
+ self.tl_cache_time = {} # 缓存时间
+ self.logger = logging.LoggerAdapter(logging.getLogger(__name__), {"vehicle_id": vehicle_id})
def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_transform):
+ """计算车辆与交通灯的夹角"""
vehicle_forward = vehicle_transform.get_forward_vector()
vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y])
vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward)
@@ -711,19 +853,24 @@ def _calculate_angle_between_vehicle_and_light(self, vehicle_transform, light_tr
light_dir = light_dir / np.linalg.norm(light_dir)
angle = math.acos(np.clip(np.dot(vehicle_forward, light_dir), -1.0, 1.0))
- angle = math.degrees(angle)
- return angle
+ return math.degrees(angle)
def get_lane_traffic_light(self, vehicle, world):
+ """获取车道对应的交通灯（带缓存）"""
vehicle_transform = vehicle.get_transform()
vehicle_loc = vehicle_transform.location
+ current_time = time.time()
+ # 缓存检查
if self.tracked_light and is_actor_alive(self.tracked_light):
- dist = self.tracked_light.get_transform().location.distance(vehicle_loc)
- angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, self.tracked_light.get_transform())
- if dist < TRAFFIC_LIGHT_DETECTION_RANGE and angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
- return self.tracked_light
-
+ tl_id = self.tracked_light.id
+ if tl_id in self.tl_state_cache and current_time - self.tl_cache_time.get(tl_id, 0) < 1.0:
+ dist = self.tracked_light.get_transform().location.distance(vehicle_loc)
+ angle = self._calculate_angle_between_vehicle_and_light(vehicle_transform, self.tracked_light.get_transform())
+ if dist < TRAFFIC_LIGHT_DETECTION_RANGE and angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
+ return self.tracked_light
+
+ # 重新查找交通灯
traffic_lights = world.get_actors().filter("traffic.traffic_light")
valid_lights = []
@@ -737,15 +884,19 @@ def get_lane_traffic_light(self, vehicle, world):
if angle < TRAFFIC_LIGHT_ANGLE_THRESHOLD:
valid_lights.append((dist, light))
+ # 更新追踪的交通灯
if valid_lights:
valid_lights.sort(key=lambda x: x[0])
self.tracked_light = valid_lights[0][1]
- return self.tracked_light
+ self.tl_state_cache[self.tracked_light.id] = self.tracked_light.get_state()
+ self.tl_cache_time[self.tracked_light.id] = current_time
+ else:
+ self.tracked_light = None
- self.tracked_light = None
- return None
+ return self.tracked_light
def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
+ """处理交通灯逻辑"""
world = vehicle.get_world()
traffic_light = self.get_lane_traffic_light(vehicle, world)
@@ -754,10 +905,13 @@ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
self.red_light_stop_time = 0
return base_target_speed, "No Light"
+ # 获取停止线和距离
stop_line_loc = get_traffic_light_stop_line(traffic_light)
dist_to_stop_line = vehicle.get_transform().location.distance(stop_line_loc)
- if traffic_light.get_state() == carla.TrafficLightState.Green:
+ # 交通灯状态处理
+ tl_state = traffic_light.get_state()
+ if tl_state == carla.TrafficLightState.Green:
if self.is_stopped_at_red:
recovery_speed = current_speed + (base_target_speed - current_speed) * GREEN_LIGHT_ACCEL_FACTOR
target_speed = max(STOP_SPEED_THRESHOLD, recovery_speed)
@@ -767,17 +921,17 @@ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
return target_speed, "Green"
return base_target_speed, "Green"
- elif traffic_light.get_state() == carla.TrafficLightState.Yellow:
+ elif tl_state == carla.TrafficLightState.Yellow:
self.is_stopped_at_red = False
yellow_speed = max(5.0, base_target_speed * 0.3)
self.logger.warning(f"黄灯减速，目标速度：{yellow_speed:.1f}km/h")
return yellow_speed, "Yellow"
- elif traffic_light.get_state() == carla.TrafficLightState.Red:
+ elif tl_state == carla.TrafficLightState.Red:
if dist_to_stop_line > TRAFFIC_LIGHT_STOP_DISTANCE:
self.is_stopped_at_red = False
red_speed = max(2.0, current_speed * 0.1)
- self.logger.warning(f"红灯减速，距离停止线：{dist_to_stop_line:.1f}m，目标速度：{red_speed:.1f}km/h")
+ self.logger.warning(f"红灯减速，距离停止线：{dist_to_stop_line:.1f}m")
return red_speed, "Red"
else:
if current_speed <= STOP_SPEED_THRESHOLD:
@@ -794,8 +948,8 @@ def handle_traffic_light_logic(self, vehicle, current_speed, base_target_speed):
# ===================== 交通灯控制线程 ======================
def cycle_traffic_light_states(world, stop_event):
- logger = logging.getLogger(__name__)
- logger = logging.LoggerAdapter(logger, {"vehicle_id": "系统"})
+ """交通灯状态循环"""
+ logger = logging.LoggerAdapter(logging.getLogger(__name__), {"vehicle_id": "系统"})
while not stop_event.is_set():
traffic_lights = world.get_actors().filter("traffic.traffic_light")
if not traffic_lights:
@@ -840,13 +994,17 @@ def cycle_traffic_light_states(world, stop_event):
logger.info("交通灯线程停止")
-# ===================== 主函数 ======================
+# ===================== 主函数（V3.0终极版）=====================
def main():
- global current_view_vehicle_id, vehicle_agents
+ global current_view_vehicle_id, vehicle_agents, perf_stats
pygame.init()
screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
- pygame.display.set_caption(f"CARLA多车辆视角（{VEHICLE_COUNT}辆车）- V2.0 LiDAR感知 - 按1/2/3切换视角，按S切换分屏，按V切换俯视视角")
+ pygame.display.set_caption(
+ f"CARLA多车辆控制 V3.0 | 车辆数：{VEHICLE_COUNT} | "
+ f"按1/2/3切换视角 | S分屏 | V俯视 | ESC退出"
+ )
+ # 初始化核心变量
client = None
world = None
map = None
@@ -855,38 +1013,53 @@ def main():
show_split_screen = True
show_top_view = False
top_view_camera = None
+ top_view_surface = None
- # 清理函数
+ # 安全清理函数
def cleanup():
- print("\n开始清理资源...")
+ print("\n=== 开始安全清理资源 ===")
tl_stop_event.set()
+
+ # 等待交通灯线程结束
if tl_cycle_thread and tl_cycle_thread.is_alive():
tl_cycle_thread.join(timeout=2)
+ # 销毁俯视相机
if top_view_camera:
- top_view_camera.stop()
- top_view_camera.destroy()
+ safe_sensor_stop(top_view_camera, "俯视相机", logging.getLogger(__name__))
+ safe_actor_destroy(top_view_camera, "俯视相机", logging.getLogger(__name__))
+ # 销毁所有车辆
+ global vehicle_agents
for agent in vehicle_agents:
- agent.destroy()
+ try:
+ agent.destroy()
+ except Exception as e:
+ print(f"销毁车辆{agent.vehicle_id}忽略异常：{str(e)[:50]}")
+ # 清理残留actor
if world:
for actor in world.get_actors():
if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
- if is_actor_alive(actor):
- actor.destroy()
+ safe_actor_destroy(actor, actor.type_id, logging.getLogger(__name__))
+ # 退出pygame
pygame.quit()
- print("资源清理完成")
+ print("=== 资源清理完成 ===")
- # 注册退出回调
+ # 注册退出处理
import atexit
import signal
atexit.register(cleanup)
- signal.signal(signal.SIGINT, lambda sig, frame: sys.exit(0))
+
+ def signal_handler(sig, frame):
+ print("\n接收到退出信号，开始清理...")
+ cleanup()
+ sys.exit(0)
+ signal.signal(signal.SIGINT, signal_handler)
try:
- # 连接CARLA
+ # 连接CARLA服务器
client = carla.Client(CARLA_HOST, CARLA_PORT)
client.set_timeout(CARLA_TIMEOUT)
try:
@@ -894,83 +1067,92 @@ def cleanup():
print("成功加载Town04地图")
except Exception as e:
world = client.get_world()
- print(f"警告：Town04地图加载失败（{e}），使用当前地图")
+ print(f"警告：Town04加载失败（{e}），使用当前地图")
map = world.get_map()
# 清理残留演员
print("清理残留演员...")
for actor in world.get_actors():
if actor.type_id.startswith(("vehicle.", "walker.", "sensor.")):
- if is_actor_alive(actor):
- actor.destroy()
- time.sleep(3.0)
- print("清理完成")
-
- # 自动获取地图的第一个出生点作为基准（避免手动坐标无效）
- base_location = None
- all_spawn_points = map.get_spawn_points()
- if all_spawn_points:
- base_location = all_spawn_points[0].location
- print(f"使用地图第一个出生点作为基准：({base_location.x:.1f}, {base_location.y:.1f})")
- else:
- base_location = carla.Location(x=220.0, y=150.0, z=0.5)
+ safe_actor_destroy(actor, actor.type_id, logging.getLogger(__name__))
+ time.sleep(2.0)
- # 获取有效的出生点
- print(f"获取{VEHICLE_COUNT}个有效出生点...")
+ # 获取出生点
+ base_location = map.get_spawn_points()[0].location if map.get_spawn_points() else carla.Location(x=220.0, y=150.0)
+ print(f"基准出生点：({base_location.x:.1f}, {base_location.y:.1f})")
+
valid_spawn_points = get_valid_spawn_points(map, VEHICLE_COUNT, base_location)
for i, sp in enumerate(valid_spawn_points):
- print(f" 出生点{i+1}：({sp.location.x:.1f},{sp.location.y:.1f})")
+ print(f"出生点{i+1}：({sp.location.x:.1f}, {sp.location.y:.1f})")
# 生成车辆
- print(f"\n分步生成车辆（间隔{SPAWN_INTERVAL}秒）...")
+ print(f"\n分步生成{VEHICLE_COUNT}辆车辆...")
for i in range(VEHICLE_COUNT):
vehicle_model = VEHICLE_MODELS[i % len(VEHICLE_MODELS)]
base_speed = BASE_SPEEDS[i % len(BASE_SPEEDS)]
spawn_point = valid_spawn_points[i]
try:
- print(f"\n生成车辆{i+1}（车型：{vehicle_model}）...")
agent = VehicleAgent(world, map, i+1, spawn_point, vehicle_model, base_speed)
- vehicle_agents.append(agent)
- print(f"车辆{i+1}生成成功！已挂载LiDAR+碰撞传感器")
+ if agent.is_alive:
+ vehicle_agents.append(agent)
+ print(f"车辆{i+1}生成成功")
+ else:
+ print(f"车辆{i+1}生成失败")
except Exception as e:
- print(f"车辆{i+1}生成失败：{e}")
+ print(f"车辆{i+1}生成异常：{str(e)[:100]}")
time.sleep(SPAWN_INTERVAL)
if len(vehicle_agents) == 0:
raise RuntimeError("无车辆生成成功，仿真终止")
- print(f"\n共生成{len(vehicle_agents)}辆车辆！V2.0 LiDAR感知+障碍物避障功能已启用")
-
- # 创建全局俯视相机
+ # 创建俯视相机
try:
top_view_bp = world.get_blueprint_library().find("sensor.camera.rgb")
top_view_bp.set_attribute("image_size_x", str(WINDOW_WIDTH))
top_view_bp.set_attribute("image_size_y", str(WINDOW_HEIGHT))
- top_view_bp.set_attribute("fov", str(90))
+ top_view_bp.set_attribute("fov", "90")
+
top_view_transform = carla.Transform(
vehicle_agents[0].vehicle.get_transform().location + carla.Location(z=50),
carla.Rotation(pitch=-90)
)
top_view_camera = world.spawn_actor(top_view_bp, top_view_transform)
- top_view_surface = None
- top_view_camera.listen(lambda image: globals().update({
- "top_view_surface": pygame.surfarray.make_surface(
- np.swapaxes(np.array(image.raw_data).reshape((image.height, image.width, 4))[:, :, :3][:, :, ::-1], 0, 1)
- )
- }))
- except:
- print("警告：无法创建俯视相机")
+
+ def top_view_callback(image):
+ nonlocal top_view_surface
+ try:
+ array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))
+ array = array[:, :, :3][:, :, ::-1]
+ array = np.swapaxes(array, 0, 1)
+ top_view_surface = pygame.surfarray.make_surface(array)
+ except:
+ pass
+
+ top_view_camera.listen(top_view_callback)
+ print("俯视相机创建成功")
+ except Exception as e:
+ print(f"俯视相机创建失败：{e}")
+ top_view_camera = None
# 启动交通灯线程
tl_cycle_thread = threading.Thread(target=cycle_traffic_light_states, args=(world, tl_stop_event), daemon=True)
tl_cycle_thread.start()
- print("交通灯线程启动")
+ print("交通灯控制线程启动")
# 主循环
clock = pygame.time.Clock()
running = True
+ executor = ThreadPoolExecutor(max_workers=VEHICLE_COUNT) # 复用线程池，提升性能
+
+ print("\n=== 仿真开始 ===")
+ print("操作说明：")
+ print(" 1/2/3 - 切换单车辆视角")
+ print(" S - 切换分屏视角")
+ print(" V - 切换俯视视角")
+ print(" ESC - 退出仿真")
+ print("="*50)
while running:
# 事件处理
@@ -1002,68 +1184,54 @@ def cleanup():
# 清空屏幕
screen.fill((0, 0, 0))
- if show_top_view:
- if top_view_surface:
- screen.blit(top_view_surface, (0, 0))
+ # 视角渲染
+ if show_top_view and top_view_surface:
+ screen.blit(top_view_surface, (0, 0))
elif show_split_screen:
- if len(vehicle_agents) == 1:
- agent = vehicle_agents[0]
- if agent.sensors.image_surface:
- surface = pygame.transform.scale(agent.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
- screen.blit(surface, (0, 0))
- elif len(vehicle_agents) == 2:
- agent1 = vehicle_agents[0]
- agent2 = vehicle_agents[1]
-
- if agent1.sensors.image_surface:
- surface1 = pygame.transform.scale(agent1.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
- screen.blit(surface1, (0, 0))
-
- if agent2.sensors.image_surface:
- surface2 = pygame.transform.scale(agent2.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT))
- screen.blit(surface2, (WINDOW_WIDTH//2, 0))
- elif len(vehicle_agents) >= 3:
- agent1 = vehicle_agents[0]
- agent2 = vehicle_agents[1]
- agent3 = vehicle_agents[2]
-
- if agent1.sensors.image_surface:
- surface1 = pygame.transform.scale(agent1.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
- screen.blit(surface1, (0, 0))
-
- if agent2.sensors.image_surface:
- surface2 = pygame.transform.scale(agent2.sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
- screen.blit(surface2, (WINDOW_WIDTH//2, 0))
-
- if agent3.sensors.image_surface:
- surface3 = pygame.transform.scale(agent3.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
- screen.blit(surface3, (0, WINDOW_HEIGHT//2))
+ # 分屏渲染
+ if len(vehicle_agents) >= 1 and vehicle_agents[0].sensors and vehicle_agents[0].sensors.image_surface:
+ surf1 = pygame.transform.scale(vehicle_agents[0].sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surf1, (0, 0))
+
+ if len(vehicle_agents) >= 2 and vehicle_agents[1].sensors and vehicle_agents[1].sensors.image_surface:
+ surf2 = pygame.transform.scale(vehicle_agents[1].sensors.image_surface, (WINDOW_WIDTH//2, WINDOW_HEIGHT//2))
+ screen.blit(surf2, (WINDOW_WIDTH//2, 0))
+
+ if len(vehicle_agents) >= 3 and vehicle_agents[2].sensors and vehicle_agents[2].sensors.image_surface:
+ surf3 = pygame.transform.scale(vehicle_agents[2].sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT//2))
+ screen.blit(surf3, (0, WINDOW_HEIGHT//2))
else:
- target_agent = None
- for agent in vehicle_agents:
- if agent.vehicle_id == current_view_vehicle_id:
- target_agent = agent
- break
-
- if target_agent and target_agent.sensors.image_surface:
- surface = pygame.transform.scale(target_agent.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
- screen.blit(surface, (0, 0))
-
- # 更新车辆状态
- with ThreadPoolExecutor(max_workers=VEHICLE_COUNT) as executor:
- futures = [executor.submit(agent.update) for agent in vehicle_agents]
- for future in as_completed(futures):
- try:
- future.result()
- except Exception as e:
- print(f"车辆更新异常：{e}")
+ # 单车辆视角
+ target_agent = next((a for a in vehicle_agents if a.vehicle_id == current_view_vehicle_id), None)
+ if target_agent and target_agent.sensors and target_agent.sensors.image_surface:
+ surf = pygame.transform.scale(target_agent.sensors.image_surface, (WINDOW_WIDTH, WINDOW_HEIGHT))
+ screen.blit(surf, (0, 0))
+
+ # 更新车辆状态（复用线程池）
+ futures = []
+ for agent in vehicle_agents:
+ if agent.is_alive:
+ futures.append(executor.submit(agent.update))
+
+ for future in as_completed(futures):
+ try:
+ future.result()
+ except Exception as e:
+ print(f"车辆更新异常：{str(e)[:50]}")
+
+ # 性能监控
+ perf_stats["frame_count"] += 1
+ print_performance_stats()
# 刷新屏幕
pygame.display.flip()
clock.tick(30)
+ # 关闭线程池
+ executor.shutdown(wait=True)
+
except Exception as e:
- print(f"仿真异常：{e}")
+ print(f"\n仿真异常：{e}")
traceback.print_exc()
finally:
cleanup()