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