diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/run_carla.py similarity index 57% rename from src/self_driving_car_navigation/carla_environment.py rename to src/self_driving_car_navigation/run_carla.py index 5d6cc466b4..480ac95f99 100644 --- a/src/self_driving_car_navigation/carla_environment.py +++ b/src/self_driving_car_navigation/run_carla.py @@ -4,8 +4,11 @@ import time import sys import random +import pygame +import os from queue import Queue from gym import spaces +from collections import deque class CarlaEnvironment(gym.Env): def __init__(self): @@ -21,7 +24,7 @@ def __init__(self): self.lidar = None self.imu = None self.spawn_points = [] - # TM核心配置(0.9.11专用) + # TM核心配置(0.9.11专用 - 仅保留全局配置) self.traffic_manager = None self.tm_port = 8000 self.tm_seed = 0 # 固定TM种子,保证行为一致 @@ -29,13 +32,10 @@ def __init__(self): self.image_queue = Queue(maxsize=1) self.lidar_queue = Queue(maxsize=1) self.imu_queue = Queue(maxsize=1) - # 车辆控制相关 - self.vehicle_control = carla.VehicleControl() - self.last_steer = 0.0 - # 连接CARLA + # 连接CARLA(强化严格同步模式) self._connect_carla() - # 初始化TM(适配0.9.11 API) + # 初始化TM(仅用0.9.11确实验证的全局API) self._init_traffic_manager() # 定义观测空间 self.observation_space = gym.spaces.Dict({ @@ -43,33 +43,38 @@ def __init__(self): 'lidar_distances': gym.spaces.Box(low=0, high=50, shape=(360,), dtype=np.float32), 'imu': gym.spaces.Box(low=-10, high=10, shape=(6,), dtype=np.float32) }) - # 获取有效生成点(仅保留道路上的点,修改:补充到60个以适配60辆NPC) + # 获取有效生成点(仅保留道路上的点,补充到60个以适配60辆NPC) self.spawn_points = self._get_valid_road_spawn_points() print(f"[场景初始化] 有效道路生成点数量: {len(self.spawn_points)}") sys.stdout.flush() def _connect_carla(self): - """连接CARLA(增加版本检查)""" + """连接CARLA(极致同步配置)""" retry_count = 3 for i in range(retry_count): try: print(f"[CARLA连接] 尝试第{i+1}次连接(localhost:2000)...") self.client = carla.Client('localhost', 2000) - self.client.set_timeout(20.0) + self.client.set_timeout(30.0) self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() - # 同步模式配置(0.9.11最优参数) + # 终极同步配置(消除所有帧差) self.settings = self.world.get_settings() - self.settings.synchronous_mode = True # 启用同步模式 - self.settings.fixed_delta_seconds = 1/20 # 降低帧率,提升稳定性 - self.settings.no_rendering_mode = False # 必须开启渲染,否则TM可能失效 + self.settings.synchronous_mode = True + self.settings.fixed_delta_seconds = 1/60 # 60FPS仿真,物理更平滑 + self.settings.no_rendering_mode = False + self.settings.substepping = True + self.settings.max_substep_delta_time = 0.003 # 更小的子步(3ms),过滤微米级抖动 + self.settings.max_substeps = 30 self.world.apply_settings(self.settings) + time.sleep(1.5) # 延长等待时间,确保配置完全生效 + current_settings = self.world.get_settings() + print(f"[同步模式] 生效状态: {current_settings.synchronous_mode}, 固定帧间隔: {current_settings.fixed_delta_seconds}s") # 双重清理 self._clear_all_non_ego_actors() - time.sleep(0.5) - self.world.tick() # 显式推进仿真帧 + self.world.tick() self._clear_all_non_ego_actors() # 版本检查 @@ -85,84 +90,31 @@ def _connect_carla(self): time.sleep(2) def _init_traffic_manager(self): - """初始化交通管理器(严格适配0.9.11 API)""" + """初始化交通管理器(强制同步)""" self.traffic_manager = self.client.get_trafficmanager(self.tm_port) - # 全局TM参数(0.9.11支持的全局方法) - self.traffic_manager.set_global_distance_to_leading_vehicle(2.0) # 跟车距离(float) - self.traffic_manager.set_synchronous_mode(True) # 同步模式(bool) - self.traffic_manager.set_random_device_seed(self.tm_seed) # 随机种子(int) - self.traffic_manager.global_percentage_speed_difference(0.0) # 全速行驶(float) - # 混合物理模式(0.9.11支持) + self.traffic_manager.set_global_distance_to_leading_vehicle(2.0) + self.traffic_manager.set_synchronous_mode(True) + self.traffic_manager.set_random_device_seed(self.tm_seed) + self.traffic_manager.global_percentage_speed_difference(0.0) self.traffic_manager.set_hybrid_physics_mode(True) self.traffic_manager.set_hybrid_physics_radius(50.0) - print("[TM配置] 交通管理器初始化完成(适配0.9.11 API)") + print("[TM配置] 交通管理器同步模式已启用") def _set_actor_tm_params(self, actor): - """为单个Actor设置TM参数(核心修改:提升主车辆速度,遵守交通规则)""" + """彻底移除所有Actor级TM配置""" if not self._is_actor_alive(actor): return - try: - # 关键修改:设置为0%忽略交通规则,使车辆遵守红绿灯和标志 - self.traffic_manager.ignore_lights_percentage(actor, 0.0) # 不忽略交通灯 - self.traffic_manager.ignore_signs_percentage(actor, 0.0) # 不忽略交通标志 - self.traffic_manager.ignore_walkers_percentage(actor, 0.0) # 不忽略行人 - # 允许变道(float百分比) - self.traffic_manager.allow_vehicle_lane_change(actor, 100.0) - - # 核心修改:提升速度参数(区分主车辆和NPC) - if actor.attributes.get('role_name') == 'ego_vehicle': - # 主车辆:速度限制因子提升到1.8(超速80%),最高速度设为100km/h - self.traffic_manager.set_speed_limit_factor(actor, 1.8) - self.traffic_manager.set_speed_limit(actor, 100.0) - else: - # NPC车辆:保持原有参数(可选:也可适当提升) - self.traffic_manager.set_speed_limit_factor(actor, 1.2) - self.traffic_manager.set_speed_limit(actor, 60.0) - - except Exception as e: - print(f"[TM Actor配置警告] Actor ID {actor.id}: {str(e)}") - - def _update_vehicle_steering(self, vehicle): - """更新车辆转向角,使轮胎转动跟随车辆运动""" - if not self._is_actor_alive(vehicle): - return - - try: - # 获取车辆当前速度和变换 - velocity = vehicle.get_velocity() - speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) - - # 获取车辆的物理控制 - physics_control = vehicle.get_physics_control() - - # 根据自动驾驶的控制指令获取转向角 - control = vehicle.get_control() - - # 平滑转向角变化,避免突变 - steer_factor = 0.3 # 转向灵敏度 - self.last_steer = self.last_steer * (1 - steer_factor) + control.steer * steer_factor - - # 应用转向角到所有车轮 - for wheel in physics_control.wheels: - if wheel.type == carla.WheelType.Front: # 只控制前轮转向 - wheel.steer_angle = self.last_steer * 70 # 70度最大转向角 - - # 应用物理控制 - vehicle.apply_physics_control(physics_control) - - except Exception as e: - print(f"[车辆转向更新错误] {str(e)}") + pass def _get_valid_road_spawn_points(self): - """过滤生成点:仅保留道路网络上的有效点(修改:补充到60个以适配60辆NPC)""" + """过滤生成点:仅保留道路网络上的有效点""" map = self.world.get_map() valid_points = [] for sp in map.get_spawn_points(): - # 获取生成点对应的道路点 waypoint = map.get_waypoint(sp.location) - if waypoint and waypoint.road_id != -1: # 确保在道路上 + if waypoint and waypoint.road_id != -1: valid_points.append(sp) - # 若有效点不足60,补充随机道路点(修改:从30改为60) + # 补充到60个 if len(valid_points) < 60: for _ in range(60 - len(valid_points)): random_loc = self.world.get_random_location_from_navigation() @@ -216,13 +168,12 @@ def _clear_all_non_ego_actors(self): if "has been destroyed" not in str(e) and "not found" not in str(e): print(f"[销毁Actor警告] {str(e)}") - self.world.tick() # 显式推进仿真帧 + self.world.tick() print(f"[清理] 车辆{cleared_count['vehicle']} | 自行车{cleared_count['bicycle']} | 静态车辆{cleared_count['static_vehicle']}") def process_image(self, image): - """处理摄像头数据 - 修复frame属性访问问题""" + """处理摄像头数据""" try: - # 检查是否为有效的图像对象 if not hasattr(image, 'raw_data') or not hasattr(image, 'height') or not hasattr(image, 'width'): print("[图像处理错误] 无效的图像数据") return @@ -236,9 +187,8 @@ def process_image(self, image): print(f"[图像处理错误] {str(e)}") def process_lidar(self, data): - """处理激光雷达数据 - 修复frame属性访问问题""" + """处理激光雷达数据""" try: - # 检查是否为有效的激光雷达数据 if not hasattr(data, 'raw_data'): print("[激光雷达处理错误] 无效的激光雷达数据") return @@ -261,9 +211,8 @@ def process_lidar(self, data): print(f"[激光雷达处理错误] {str(e)}") def process_imu(self, data): - """处理IMU数据 - 修复frame属性访问问题""" + """处理IMU数据""" try: - # 检查是否为有效的IMU数据 if not hasattr(data, 'accelerometer') or not hasattr(data, 'gyroscope'): print("[IMU处理错误] 无效的IMU数据") return @@ -279,7 +228,7 @@ def process_imu(self, data): print(f"[IMU处理错误] {str(e)}") def reset(self): - """重置环境(修改:生成主车辆+60辆NPC)""" + """重置环境""" self.close() time.sleep(1.0) self._clear_all_non_ego_actors() @@ -287,28 +236,44 @@ def reset(self): if self.vehicle: self._spawn_sensors() - # 启用物理模拟 self.vehicle.set_simulate_physics(True) - # 主车辆自动驾驶(延迟绑定+TM参数配置) + # 优化车辆物理参数(彻底消除微小抖动) + self._optimize_vehicle_physics() time.sleep(0.5) - self.vehicle.set_autopilot(True, self.tm_port) # 启用自动控制指令输入 - self._set_actor_tm_params(self.vehicle) # 为主车辆配置TM参数 - # 生成NPC车辆(核心修改:从20辆改为60辆) + self.vehicle.set_autopilot(True, self.tm_port) self._spawn_npcs(60) self._clear_all_non_ego_actors() - # 多次同步,确保物理生效 - for _ in range(5): - self.world.tick() # 显式推进仿真帧 - time.sleep(0.2) + # 多次同步,物理稳定(延长时间) + for _ in range(15): + self.world.tick() + time.sleep(0.05) print(f"[环境重置] 完成,主车辆1辆,NPC车辆{len(self.npc_vehicles)}辆") return self.get_observation() + def _optimize_vehicle_physics(self): + """终极物理优化:彻底消除车辆微小震动""" + if not self._is_actor_alive(self.vehicle): + return + try: + physics_control = self.vehicle.get_physics_control() + # 进一步降低悬挂刚度(从1000→800),减少微米级震动 + for wheel in physics_control.wheels: + wheel.suspension_stiffness = 800.0 # 更低的刚度,过滤路面微小颠簸 + wheel.suspension_damping = 250.0 # 更高的阻尼,快速衰减震动 + wheel.suspension_compression = 300.0 # 增加压缩阻尼 + wheel.max_suspension_travel = 0.08 # 减小悬挂行程,避免过度晃动 + wheel.friction_slip = 1.2 # 增加轮胎抓地力,减少打滑抖动 + self.vehicle.apply_physics_control(physics_control) + print("[物理优化] 车辆悬挂+轮胎参数已终极优化,消除微小震动") + except Exception as e: + print(f"[物理优化警告] {str(e)}") + def _spawn_vehicle(self): - """生成主车辆(特斯拉Model3)""" + """生成主车辆""" self._safe_destroy_actor(self.vehicle) - self.world.tick() # 显式推进仿真帧 + self.world.tick() vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3') vehicle_bp.set_attribute('color', '255,0,0') @@ -321,19 +286,18 @@ def _spawn_vehicle(self): for spawn_point in self.spawn_points[:10]: self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if self.vehicle: - self.vehicle.set_simulate_physics(True) # 启用物理模拟 + self.vehicle.set_simulate_physics(True) print(f"[主车辆生成] 成功(ID: {self.vehicle.id})") return raise RuntimeError("主车辆生成失败,请重启CARLA") def _spawn_npcs(self, count): - """生成NPC车辆(0.9.11关键:生成后延迟启用自动驾驶+单个Actor配置TM)""" + """生成NPC车辆""" if not self.spawn_points: print("[NPC生成] 无可用生成点") return - # 过滤主车辆附近的点(修改:将距离从20米改为15米,释放更多生成点) ego_transform = self.vehicle.get_transform() available_spawn_points = [] for sp in self.spawn_points: @@ -341,21 +305,19 @@ def _spawn_npcs(self, count): sp.location.x - ego_transform.location.x, sp.location.y - ego_transform.location.y ]) - if distance > 15.0: # 从20米→15米,增加可用生成点数量 + if distance > 15.0: available_spawn_points.append(sp) if len(available_spawn_points) < count: count = len(available_spawn_points) print(f"[NPC生成] 可用点不足,生成{count}辆") - # 随机车辆蓝图 vehicle_bps = [bp for bp in self.blueprint_library.filter('vehicle.*') if bp.has_attribute('color') and bp.id != 'vehicle.tesla.model3'] random.shuffle(vehicle_bps) if not vehicle_bps: vehicle_bps = self.blueprint_library.filter('vehicle.*') - # 生成NPC spawned_count = 0 for i, spawn_point in enumerate(random.sample(available_spawn_points, count)): bp = vehicle_bps[i % len(vehicle_bps)] @@ -366,19 +328,16 @@ def _spawn_npcs(self, count): npc_vehicle = self.world.try_spawn_actor(bp, spawn_point) if npc_vehicle: - npc_vehicle.set_simulate_physics(True) # 启用物理模拟 - # 0.9.11核心:生成后延迟0.1秒启用自动驾驶,让物理模拟生效 - time.sleep(0.1) - npc_vehicle.set_autopilot(True, self.tm_port) # 启用自动控制指令输入 - # 为单个NPC配置TM参数(关键:解决不动问题) - self._set_actor_tm_params(npc_vehicle) + npc_vehicle.set_simulate_physics(True) + time.sleep(0.05) + npc_vehicle.set_autopilot(True, self.tm_port) self.npc_vehicles.append(npc_vehicle) spawned_count += 1 if spawned_count % 5 == 0: - self.world.tick() # 显式推进仿真帧 + self.world.tick() - self.world.tick() # 显式推进仿真帧 + self.world.tick() print(f"[NPC生成] 成功生成{spawned_count}辆(目标:{count}辆)") def _spawn_sensors(self): @@ -400,8 +359,8 @@ def _spawn_sensors(self): lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast') lidar_bp.set_attribute('channels', '32') lidar_bp.set_attribute('range', '50') - lidar_bp.set_attribute('points_per_second', '100000') - lidar_bp.set_attribute('rotation_frequency', '10') + lidar_bp.set_attribute('points_per_second', '200000') + lidar_bp.set_attribute('rotation_frequency', '60') # 与仿真帧率一致 self.lidar = self.world.spawn_actor( lidar_bp, carla.Transform(carla.Location(x=0.0, z=2.0)), attach_to=self.vehicle ) @@ -413,12 +372,12 @@ def _spawn_sensors(self): imu_bp, carla.Transform(), attach_to=self.vehicle ) self.imu.listen(self.process_imu) - print("[传感器] 初始化完成") + print("[传感器] 初始化完成(帧率与仿真同步)") def get_observation(self): """获取观测数据""" while self.image_queue.empty() or self.lidar_queue.empty() or self.imu_queue.empty(): - time.sleep(0.01) + time.sleep(0.001) return { 'image': self.image_queue.get(), 'lidar_distances': self.lidar_queue.get(), @@ -439,33 +398,22 @@ def get_obstacle_directions(self, lidar_distances): } def step(self, action=None): - """环境交互步骤(0.9.11同步关键)""" - # 同步世界(TM会自动同步) - self.world.tick() # 显式推进仿真帧 - - # 更新主车辆轮胎转向 - if self._is_actor_alive(self.vehicle): - self._update_vehicle_steering(self.vehicle) - - # 更新NPC车辆轮胎转向 - for npc in self.npc_vehicles: - if self._is_actor_alive(npc): - self._update_vehicle_steering(npc) + """环境交互步骤""" + self.world.tick() # 清理无效NPC self.npc_vehicles = [npc for npc in self.npc_vehicles if self._is_actor_alive(npc)] - # 打印NPC速度(调试用) - if random.random() < 0.1: # 10%概率打印 + # 打印NPC速度(调试用,降低频率) + if random.random() < 0.02: for npc in self.npc_vehicles[:1]: try: velocity = npc.get_velocity() - speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) * 3.6 # m/s → km/h + speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) * 3.6 print(f"[NPC速度] ID:{npc.id} 速度:{speed:.1f}km/h") except Exception: pass - # 获取观测 observation = self.get_observation() reward = 1.0 done = False @@ -473,25 +421,19 @@ def step(self, action=None): def close(self): """清理资源""" - # 停止TM if self.traffic_manager: try: self.traffic_manager.set_synchronous_mode(False) except Exception as e: print(f"[TM清理警告] {str(e)}") - # 销毁传感器 for sensor in [self.camera, self.lidar, self.imu]: self._safe_destroy_actor(sensor) - # 销毁NPC for npc in self.npc_vehicles: self._safe_destroy_actor(npc) self.npc_vehicles = [] - # 销毁主车辆 self._safe_destroy_actor(self.vehicle) self.vehicle = None - # 最后清理 self._clear_all_non_ego_actors() - # 清空队列 for q in [self.image_queue, self.lidar_queue, self.imu_queue]: while not q.empty(): q.get() @@ -499,30 +441,196 @@ def close(self): if self.settings: try: self.settings.synchronous_mode = False + self.settings.substepping = False self.world.apply_settings(self.settings) except Exception as e: print(f"[世界设置恢复警告] {str(e)}") print("[资源清理] 所有资源已销毁") + def init_spectator_smoother(self, window_size=15): + """初始化镜头平滑器(增大滑动窗口到15帧)""" + self.vehicle_pose_buffer = deque(maxlen=window_size) # 15帧缓存,过滤更多高频抖动 + self.window_size = window_size + # 初始化加权平均权重(近期帧权重更高,兼顾平滑和响应) + self.weights = np.linspace(0.1, 1.0, window_size) # 权重从0.1→1.0递增 + self.weights /= np.sum(self.weights) # 归一化 + + def update_spectator_ultra_smooth(self, spectator): + """ + 终极平滑镜头更新:加权滑动平均+微米级死区+完全锁定旋转 + """ + if not self._is_actor_alive(self.vehicle): + return + + # 1. 获取同步帧快照的车辆位姿(仅用快照,杜绝异步) + snapshot = self.world.get_snapshot() + vehicle_snapshot = snapshot.find(self.vehicle.id) + if not vehicle_snapshot: + return + current_pose = vehicle_snapshot.get_transform() + + # 2. 初始化关键变量 + avg_yaw = current_pose.rotation.yaw + avg_pose = current_pose + + # 3. 加入滑动缓存并计算「加权平均」位姿(核心优化) + self.vehicle_pose_buffer.append(current_pose) + if len(self.vehicle_pose_buffer) >= self.window_size: + # 提取缓存中的位姿 + poses = list(self.vehicle_pose_buffer) + count = len(poses) + + # 加权平均位置(近期帧权重更高) + avg_loc = carla.Location() + for i in range(count): + weight = self.weights[i] + avg_loc.x += poses[i].location.x * weight + avg_loc.y += poses[i].location.y * weight + avg_loc.z += poses[i].location.z * weight # Z轴也加权平均 + + # 加权平均Yaw角(处理360度环绕) + yaws = [pose.rotation.yaw for pose in poses] + yaw_rads = np.radians(yaws) + # 加权正弦和余弦 + weighted_sin = np.sum(np.sin(yaw_rads) * self.weights[:count]) + weighted_cos = np.sum(np.cos(yaw_rads) * self.weights[:count]) + avg_yaw = np.degrees(np.arctan2(weighted_sin, weighted_cos)) + + # 构建平均位姿 + avg_pose = carla.Transform(avg_loc, carla.Rotation(pitch=0, yaw=avg_yaw, roll=0)) + + # 4. 模拟父级绑定+Z轴强制锁定 + relative_loc = carla.Location(x=-5.0, y=0.0, z=2.0) + target_loc = avg_pose.transform(relative_loc) + target_loc.z = avg_pose.location.z + 2.0 # 强制锁定Z轴,不随任何波动 + + # 5. 镜头旋转:完全锁定(仅Yaw跟随平均位姿) + target_rot = carla.Rotation( + pitch=-10.0, # 完全固定,不参与任何平滑 + yaw=avg_yaw, + roll=0.0 # 完全固定 + ) + + # 6. EMA平滑+微米级死区过滤(终极去抖) + current_transform = spectator.get_transform() + alpha = 0.03 # 极致平滑系数(更小,更稳定) + pos_deadzone = 0.005 # 微米级死区(5mm内波动不更新) + + # 位置平滑(带死区) + loc_diff = np.array([ + target_loc.x - current_transform.location.x, + target_loc.y - current_transform.location.y, + target_loc.z - current_transform.location.z + ]) + loc_diff_mag = np.linalg.norm(loc_diff) + + if loc_diff_mag > pos_deadzone: + final_loc = carla.Location( + x=alpha * target_loc.x + (1 - alpha) * current_transform.location.x, + y=alpha * target_loc.y + (1 - alpha) * current_transform.location.y, + z=target_loc.z # Z轴直接锁定 + ) + else: + # 死区内不更新,保持当前位置 + final_loc = current_transform.location + + # 旋转平滑(仅Yaw,带死区) + yaw_diff = target_rot.yaw - current_transform.rotation.yaw + yaw_diff = (yaw_diff + 180) % 360 - 180 # 归一化 + rot_deadzone = 0.02 # 0.02度死区 + + if abs(yaw_diff) > rot_deadzone: + final_yaw = current_transform.rotation.yaw + alpha * yaw_diff + final_yaw = final_yaw % 360 + else: + final_yaw = current_transform.rotation.yaw + + final_rot = carla.Rotation( + pitch=-10.0, # 完全固定 + yaw=final_yaw, + roll=0.0 # 完全固定 + ) + + # 7. 更新镜头(仅一次,同步帧内完成) + spectator.set_transform(carla.Transform(final_loc, final_rot)) + -if __name__ == "__main__": - # 测试环境 +def run_simulation(): + pygame.init() + env = None try: + print("\n[CARLA连接] 创建环境...") env = CarlaEnvironment() - print("环境初始化完成,开始测试...") - obs = env.reset() - print(f"观测数据:图像{obs['image'].shape},激光雷达{obs['lidar_distances'].shape},IMU{obs['imu'].shape}") - # 运行600步(约30秒) - for i in range(600): - obs, reward, done, _ = env.step() - if i % 50 == 0: - obstacle_info = env.get_obstacle_directions(obs['lidar_distances']) - print(f"第{i}步 - 前向距离:{obstacle_info['front']:.2f}m,NPC数量:{len(env.npc_vehicles)}") - time.sleep(0.05) - env.close() - print("测试完成") + + # 初始化镜头平滑器(15帧加权滑动窗口) + env.init_spectator_smoother(window_size=15) + + print("\n[环境重置] 生成车辆和传感器...") + env.reset() + + if not env.vehicle or not env.vehicle.is_alive: + raise RuntimeError("车辆生成失败,请检查CARLA是否正常运行") + print(f"[车辆状态] 生成成功(ID: {env.vehicle.id}),已启用自动驾驶") + + clock = pygame.time.Clock() + spectator = env.world.get_spectator() + + # 初始化镜头+填充缓存(延长初始化时间) + env.world.tick() + for _ in range(env.window_size * 2): # 填充2倍窗口,确保加权平均生效 + env.update_spectator_ultra_smooth(spectator) + env.world.tick() + + print("\n[仿真开始] 车辆将沿车道行驶,按Ctrl+C退出...") + sys.stdout.flush() + + step = 0 + obstacle_distances = {'front': 0, 'rear': 0, 'left': 0, 'right': 0} + while True: + # 处理pygame事件 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + raise KeyboardInterrupt + + # 严格同步的仿真帧推进 + env.world.tick() + + # 终极平滑的镜头更新 + env.update_spectator_ultra_smooth(spectator) + + # 极低频率获取观测(减少性能消耗) + if step % 3 == 0: + observation = env.get_observation() + obstacle_distances = env.get_obstacle_directions(observation['lidar_distances']) + + # 极低频率打印(每2秒打印一次,减少IO抖动) + if step % 120 == 0: + print(f"\n[步骤 {step}] 障碍物距离 - 前{obstacle_distances['front']:.1f}m | 后{obstacle_distances['rear']:.1f}m | " + f"左{obstacle_distances['left']:.1f}m | 右{obstacle_distances['right']:.1f}m") + sys.stdout.flush() + + # 锁定渲染帧率(与仿真帧率一致,避免波动) + clock.tick_busy_loop(60) # 更精准的帧率锁定 + step += 1 + + except KeyboardInterrupt: + print("\n[用户终止] 收到退出信号") except Exception as e: - print(f"测试出错:{str(e)}") - if 'env' in locals(): + print(f"\n[仿真错误] {str(e)}") + import traceback + traceback.print_exc() + sys.stdout.flush() + finally: + if env is not None: + print("\n[资源清理] 销毁资源...") env.close() - sys.exit(1) \ No newline at end of file + pygame.quit() + print("\n[程序退出]") + +if __name__ == "__main__": + print("="*60) + print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}") + print(f"[Python解释器] {sys.executable}") + print("="*60) + sys.stdout.flush() + run_simulation() \ No newline at end of file diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py deleted file mode 100644 index 312538d685..0000000000 --- a/src/self_driving_car_navigation/run_simulation.py +++ /dev/null @@ -1,121 +0,0 @@ -import torch -import time -import sys -import pygame -import carla -import os -import sys - -# 添加当前目录到Python路径,确保可以导入_agent模块 -sys.path.append(os.path.dirname(os.path.abspath(__file__))) - -# 从_agent子目录导入CarlaEnvironment -from _agent.carla_environment import CarlaEnvironment - -print("="*60) -print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}") -print(f"[Python解释器] {sys.executable}") -print("="*60) -sys.stdout.flush() - -def set_spectator_smooth(world, vehicle, last_transform=None): - """平滑跟随视角(移植自参考代码的核心逻辑)""" - spectator = world.get_spectator() - vehicle_tf = vehicle.get_transform() - # 目标视角:车辆后上方,提供良好视野 - target_tf = carla.Transform( - vehicle_tf.transform(carla.Location(x=-8, z=3, y=0.5)), - vehicle_tf.rotation - ) - - if last_transform is None: - spectator.set_transform(target_tf) - return target_tf - - # 线性插值平滑过渡 - def lerp(a, b, t): - return a + t * (b - a) - - smooth_loc = carla.Location( - x=lerp(last_transform.location.x, target_tf.location.x, 0.1), - y=lerp(last_transform.location.y, target_tf.location.y, 0.1), - z=lerp(last_transform.location.z, target_tf.location.z, 0.1) - ) - smooth_rot = carla.Rotation( - pitch=lerp(last_transform.rotation.pitch, target_tf.rotation.pitch, 0.1), - yaw=lerp(last_transform.rotation.yaw, target_tf.rotation.yaw, 0.1), - roll=lerp(last_transform.rotation.roll, target_tf.rotation.roll, 0.1) - ) - smooth_tf = carla.Transform(smooth_loc, smooth_rot) - spectator.set_transform(smooth_tf) - return smooth_tf - -def run_simulation(): - # 初始化pygame - pygame.init() - - env = None - try: - print("\n[CARLA连接] 创建环境...") - env = CarlaEnvironment() - - print("\n[环境重置] 生成车辆和传感器...") - env.reset() - - if not env.vehicle or not env.vehicle.is_alive: - raise RuntimeError("车辆生成失败,请检查CARLA是否正常运行") - print(f"[车辆状态] 生成成功(ID: {env.vehicle.id}),已启用自动驾驶") - - # 初始化平滑视角 - last_spectator_tf = set_spectator_smooth(env.world, env.vehicle) - print("视角已切换至车辆后上方(平滑跟随模式)") - - # 初始化时钟控制帧率 - clock = pygame.time.Clock() - - print("\n[仿真开始] 车辆将沿车道行驶,按Ctrl+C退出...") - sys.stdout.flush() - - # 持续运行仿真(不限制步数) - step = 0 - while True: - # 处理pygame事件 - for event in pygame.event.get(): - if event.type == pygame.QUIT: - raise KeyboardInterrupt - - # 同步CARLA帧(关键优化:保证控制时序稳定) - env.world.tick() - - # 获取观测和障碍物信息 - observation = env.get_observation() - obstacle_distances = env.get_obstacle_directions(observation['lidar_distances']) - - # 打印状态信息 - if step % 10 == 0: # 每10步打印一次 - print(f"\n[步骤 {step}] 障碍物距离 - 前{obstacle_distances['front']:.1f}m | 后{obstacle_distances['rear']:.1f}m | " - f"左{obstacle_distances['left']:.1f}m | 右{obstacle_distances['right']:.1f}m") - sys.stdout.flush() - - # 更新平滑视角 - last_spectator_tf = set_spectator_smooth(env.world, env.vehicle, last_spectator_tf) - - # 控制帧率为30FPS - clock.tick(30) - step += 1 - - except KeyboardInterrupt: - print("\n[用户终止] 收到退出信号") - except Exception as e: - print(f"\n[仿真错误] {str(e)}") - sys.stdout.flush() - finally: - if env is not None: - print("\n[资源清理] 销毁资源...") - env.close() - # 退出pygame - pygame.quit() - print("\n[程序退出]") - -if __name__ == "__main__": - run_simulation() \ No newline at end of file