Skip to content
247 changes: 167 additions & 80 deletions src/autonomous_driving_car/MCP/v2.py
Original file line number Diff line number Diff line change
@@ -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:
Expand All @@ -44,39 +42,41 @@ 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)
: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:
"""
Expand All @@ -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服务器
Expand All @@ -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()
Expand All @@ -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:
Expand All @@ -229,7 +316,7 @@ def main():
print(f"\n[仿真错误] 未知异常:{e}")
traceback.print_exc()

# 资源清理(无论是否异常,都执行)
# 资源清理
finally:
print("\n[资源清理] 开始清理仿真资源...")

Expand Down
Loading