diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index 4580a6a70c..7912e8cb97 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -1,11 +1,12 @@ # 多模态 CARLA 导航避障系统 ## 项目简介 -本项目基于 CARLA 模拟器与神经网络技术,实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与激光雷达(LiDAR),通过多模态数据感知环境,结合智能避障算法,实现车辆自主行驶与障碍物规避功能。 +本项目基于 CARLA 模拟器与神经网络技术,实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与障碍物检测模块,通过多模态数据感知环境,结合神经网络与传统控制算法,实现车辆自主行驶与障碍物规避功能。 ## 核心功能 -- 多传感器融合感知:集成 RGB 摄像头(前视 + 第三视角)与 32 线激光雷达,全面获取环境信息 -- 智能避障算法:基于激光雷达点云数据,分区域检测障碍物,动态选择最优避障方向(左 / 右) -- 可视化监控:实时显示前视画面、第三视角跟随画面、LiDAR 鸟瞰图,叠加关键行驶数据 -- 人机交互控制:支持键盘手动干预,兼容自动 / 手动切换 +- 多模态感知系统:集成 RGB 摄像头(前视 + 第三视角)与障碍物检测器,全面获取环境信息 +- 智能避障算法:基于改进神经网络控制器与传统控制器双模式,动态检测并规避前方障碍物 +- 实时可视化:实时显示第三视角画面,叠加障碍物检测结果、车速、控制状态等关键信息 +- 双模式控制:支持神经网络模式与传统控制模式切换,可根据环境选择最优控制策略 +- 智能恢复机制:内置车辆卡住检测与自动恢复系统,保障行驶稳定性 ## 环境配置 - 操作系统:Windows 10/11 或 Ubuntu 20.04/22.04 - Python 版本:3.7 @@ -51,12 +52,26 @@ - a 手动左转向 - d 手动右转向 - r 重置转向角度(回正) - ## 系统架构 1. 环境初始化模块 -连接 CARLA 服务器(默认 localhost:2000)。 -加载 Town01 地图,设置同步模式(固定时间步长 0.05s)。 -配置天气参数(多云、无降水、太阳高度角)。 +- 连接 CARLA 服务器(默认 localhost:2000) +- 加载 Town01 地图,设置同步模式(固定时间步长 0.1s) +- 配置天气参数(云量30%、无降水、太阳高度角70°) 2. 智能体生成模块 -主车辆:特斯拉 Model3(红色),关闭自动驾驶,由自定义算法控制。 -障碍物车辆:随机生成 6 辆不同类型车辆,开启自动驾驶,分散分布在地图中。 +- 主车辆:特斯拉 Model3(红色),关闭自动驾驶,由自定义控制算法控制 +- NPC车辆:随机生成3辆不同类型车辆,开启自动驾驶,分散在50米外起始位置 +3. 传感器系统 +- 前视摄像头:90°视野,分辨率640×480,用于神经网络输入 +- 第三视角摄像头:110°视野,分辨率640×480,用于可视化监控 +- 障碍物检测器:实时检测前方±60°范围内50米内障碍物 +4. 控制系统 +- 神经网络控制器:基于改进的SimpleDrivingNetwork,融合图像与障碍物信息 +- 传统控制器:基于路点跟踪的经典控制算法,稳定性更高 +- 障碍物避障逻辑:紧急刹车、减速跟随、转向避让多级策略 +5. 可视化与监控 +- 实时显示第三视角画面 +- 可视化障碍物位置与距离 +- 显示车速、控制模式、油门/刹车/转向值 +- 安全区域标记与卡住警告 +## 联系方式 +邮箱:2985835251@qq.com \ No newline at end of file diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index f9d8ddb25d..bce974b7f0 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -14,16 +14,10 @@ import os - # 修复1: 简化的神经网络架构(增加障碍物检测通道) class SimpleDrivingNetwork(nn.Module): """ 简化的驾驶网络 - 增加障碍物感知 -======= -# 修复1: 简化的神经网络架构 -class SimpleDrivingNetwork(nn.Module): - """ - """ def __init__(self): @@ -40,7 +34,6 @@ def __init__(self): nn.AdaptiveAvgPool2d((4, 4)) ) - # 状态信息维度: 速度 + 转向历史 + 障碍物信息 state_dim = 7 # 增加障碍物相关维度 @@ -50,14 +43,6 @@ def __init__(self): nn.ReLU(), nn.Dropout(0.1), # 添加dropout防止过拟合 nn.Linear(128, 64), - - # 状态信息维度: 速度 + 转向历史 - state_dim = 4 - - # 融合层 - self.fc_layers = nn.Sequential( - nn.Linear(32 * 4 * 4 + state_dim, 64), - nn.ReLU(), nn.Linear(64, 32), nn.ReLU(), @@ -80,7 +65,6 @@ def forward(self, image, state): return torch.cat([throttle_brake, steer], dim=1) - # 新增:障碍物检测器类 class ObstacleDetector: def __init__(self, world, vehicle, max_distance=50.0): @@ -219,11 +203,6 @@ def visualize_obstacles(self, image, vehicle_transform): # 修复2: 改进的神经网络控制器(整合障碍物检测) class ImprovedNeuralController: def __init__(self, obstacle_detector=None): - -# 修复2: 改进的神经网络控制器 -class ImprovedNeuralController: - def __init__(self): - self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') print(f"使用设备: {self.device}") @@ -234,7 +213,6 @@ def __init__(self): # 控制历史,用于平滑 self.control_history = deque(maxlen=5) - # 障碍物检测器 self.obstacle_detector = obstacle_detector @@ -243,13 +221,11 @@ def __init__(self): self.last_brake = 0.0 self.last_steer = 0.0 - # 避障参数 self.emergency_brake_distance = 5.0 # 紧急刹车距离 self.safe_following_distance = 8.0 # 安全跟车距离 self.obstacle_avoidance_steer = 0.0 - def preprocess_image(self, image): """修复图像预处理""" if image is None: @@ -266,21 +242,16 @@ def preprocess_image(self, image): print(f"图像预处理错误: {e}") return torch.zeros((1, 3, 120, 160), device=self.device) - def preprocess_state(self, speed, steer_history, obstacle_info): """修复状态预处理,加入障碍物信息""" has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0 normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0 normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0 - def preprocess_state(self, speed, steer_history): - """修复状态预处理""" - state_data = [ speed / 20.0, # 归一化速度 steer_history[-1] if steer_history else 0.0, # 最近转向 steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向 - np.mean(steer_history) if steer_history else 0.0, # 平均转向 has_obstacle, # 是否有障碍物 normalized_distance, # 归一化距离 @@ -289,7 +260,7 @@ def preprocess_state(self, speed, steer_history): return torch.tensor(state_data, device=self.device).unsqueeze(0) def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed): - """应用避障逻辑""" + """应用避障逻辑 - 优化版本""" if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -299,59 +270,54 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed) # 紧急情况:前方有近距离障碍物 if distance < self.emergency_brake_distance: print(f"紧急刹车!距离障碍物: {distance:.1f}m") - return 0.0, 1.0, steer # 全力刹车 + return 0.0, 1.0, 0.0 # 紧急情况下保持直行,只刹车 # 中距离障碍物:减速并准备转向 elif distance < self.safe_following_distance: # 计算安全速度比例 - safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0) + safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0) safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio)) # 如果当前速度过高,减速 - if speed > 5.0 * safe_speed_ratio: + target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h + current_speed_kmh = speed * 3.6 + + if current_speed_kmh > target_speed: throttle = 0.0 - brake = 0.3 + brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh) + else: + throttle = 0.3 * safe_speed_ratio + brake = 0.0 # 如果障碍物在正前方,尝试轻微转向避开 - if abs(angle) < 10: # 正前方±10度内 + if abs(angle) < 15: # 正前方±15度内 # 根据障碍物距离决定转向幅度 - avoid_steer = 0.5 if angle >= 0 else -0.5 - # 平滑转向 - steer = 0.7 * steer + 0.3 * avoid_steer + avoid_factor = max(0, 1.0 - distance / self.safe_following_distance) + avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor + # 平滑转向 - 保持更多原始转向 + steer = 0.8 * steer + 0.2 * avoid_steer # 远距离障碍物:轻微调整 - elif distance < 20.0: + elif distance < 25.0: # 轻微减速 - if speed > 10.0: - throttle *= 0.8 + if speed > 8.0: + throttle *= 0.7 # 如果障碍物在正前方,轻微转向 - if abs(angle) < 15: - avoid_steer = 0.2 if angle >= 0 else -0.2 - steer = 0.8 * steer + 0.2 * avoid_steer + if abs(angle) < 20: + avoid_steer = 0.15 if angle >= 0 else -0.15 + steer = 0.9 * steer + 0.1 * avoid_steer return throttle, brake, steer def get_control(self, image, speed, steer_history, obstacle_info): """修复控制生成逻辑,加入避障""" - - np.mean(steer_history) if steer_history else 0.0 # 平均转向 - ] - return torch.tensor(state_data, device=self.device).unsqueeze(0) - - def get_control(self, image, speed, steer_history): - """修复控制生成逻辑""" - try: with torch.no_grad(): # 预处理 img_tensor = self.preprocess_image(image) - state_tensor = self.preprocess_state(speed, steer_history, obstacle_info) - state_tensor = self.preprocess_state(speed, steer_history) - - # 神经网络推理 control_output = self.model(img_tensor, state_tensor) @@ -365,13 +331,11 @@ def get_control(self, image, speed, steer_history): brake = max(0.0, min(0.5, brake)) # 限制最大刹车 steer = max(-0.5, min(0.5, steer)) # 限制转向幅度 - # 应用避障逻辑 throttle, brake, steer = self.apply_obstacle_avoidance( throttle, brake, steer, obstacle_info, speed ) - return throttle, brake, steer except Exception as e: @@ -380,30 +344,21 @@ def get_control(self, image, speed, steer_history): return 0.3, 0.0, 0.0 - # 修复5: 传统控制器作为备份(整合障碍物检测) class TraditionalController: """可靠的传统控制逻辑""" def __init__(self, world, obstacle_detector=None): - -# 修复5: 传统控制器作为备份 -class TraditionalController: - """可靠的传统控制逻辑""" - - def __init__(self, world): - self.world = world self.map = world.get_map() self.waypoint_distance = 10.0 self.last_waypoint = None - self.obstacle_detector = obstacle_detector self.emergency_brake_distance = 6.0 self.safe_following_distance = 10.0 def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info): - """传统控制器的避障逻辑""" + """传统控制器的避障逻辑 - 优化版本""" if not obstacle_info['has_obstacle']: return throttle, brake, steer @@ -421,49 +376,43 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_inf # 减速跟随 elif distance < self.safe_following_distance: # 计算所需的安全距离(基于速度) - required_distance = max(5.0, vehicle_speed * 0.3) # 0.3秒车距 + required_distance = max(5.0, vehicle_speed * 0.4) # 增加到0.4秒车距 if distance < required_distance: # 距离太近,减速 + speed_ratio = distance / required_distance if vehicle_speed > 10: throttle = 0.0 - brake = 0.4 + brake = 0.6 * (1.0 - speed_ratio) else: - throttle = 0.1 + throttle = 0.2 * speed_ratio brake = 0.0 # 如果障碍物在正前方,尝试变道 - if abs(angle) < 15: - # 获取当前车道和相邻车道 + if abs(angle) < 20: # 放宽角度范围 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 # 向左变道 + steer = -0.25 elif right_lane and right_lane.lane_type == carla.LaneType.Driving: - steer = 0.3 # 向右变道 + steer = 0.25 else: - # 没有可用的相邻车道,轻微转向避开 - steer = 0.2 if angle >= 0 else -0.2 + steer = 0.15 if angle >= 0 else -0.15 return throttle, brake, steer def get_control(self, vehicle): """基于路点的传统控制,整合避障""" - - - 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 # km/h # 获取障碍物信息 @@ -471,9 +420,6 @@ def get_control(self, vehicle): if self.obstacle_detector: obstacle_info = self.obstacle_detector.get_obstacle_info() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) - - # 获取路点 waypoint = self.map.get_waypoint(location, project_to_road=True) next_waypoints = waypoint.next(self.waypoint_distance) @@ -501,7 +447,6 @@ def get_control(self, vehicle): angle = math.atan2(local_y, local_x) steer = np.clip(angle / math.radians(45), -1.0, 1.0) - # 速度控制(基于障碍物距离调整) throttle = 0.0 brake = 0.0 @@ -556,24 +501,10 @@ def get_control(self, vehicle): throttle, brake, steer, vehicle, obstacle_info ) - # 速度控制 - if speed < 5.0: # 18 km/h - throttle = 0.6 - brake = 0.0 - elif speed < 10.0: # 36 km/h - throttle = 0.3 - brake = 0.0 - else: - throttle = 0.1 - brake = 0.1 - - return throttle, brake, steer - # CARLA初始化部分... -# CARLA初始化部分保持不变... # 连接到本地CARLA服务器,端口2000 client = carla.Client('localhost', 2000) client.set_timeout(15.0) @@ -616,17 +547,78 @@ def get_control(self, vehicle): print(f"车辆生成在位置: {spawn_point.location}") -# 生成障碍物车辆 +# 改进的NPC车辆生成和设置 +print("生成NPC车辆...") obstacle_count = 3 -for i in range(obstacle_count): - if i >= len(spawn_points): - break - other_vehicles = blueprint_library.filter('vehicle.*') - other_vehicle_bp = np.random.choice(other_vehicles) - spawn_idx = (i + 15) % len(spawn_points) - other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx]) - if other_vehicle: - other_vehicle.set_autopilot(True) +npc_vehicles = [] # 存储NPC车辆以便后续管理 + +# 获取所有可用的车辆蓝图 +vehicle_blueprints = blueprint_library.filter('vehicle.*') + +# 选择远离主车辆的出生点(避免直接堵塞) +valid_spawn_points = [] +main_spawn_location = spawn_point.location + +for point in spawn_points: + distance = main_spawn_location.distance(point.location) + # 选择距离主车辆50米以上的出生点,避免直接碰撞 + if distance > 50.0: + valid_spawn_points.append(point) + if len(valid_spawn_points) >= obstacle_count: + break + +# 如果找不到足够远的点,使用所有点 +if len(valid_spawn_points) < obstacle_count: + print("警告:找不到足够远的出生点,使用所有可用点") + valid_spawn_points = spawn_points[:obstacle_count] + +# 生成NPC车辆 +for i in range(min(obstacle_count, len(valid_spawn_points))): + try: + # 随机选择车辆类型(排除特斯拉,使场景更丰富) + available_blueprints = [bp for bp in vehicle_blueprints if 'tesla' not in bp.id] + if not available_blueprints: + available_blueprints = vehicle_blueprints + + npc_bp = random.choice(available_blueprints) + + # 设置车辆颜色 + if npc_bp.has_attribute('color'): + colors = npc_bp.get_attribute('color').recommended_values + if colors: + npc_bp.set_attribute('color', random.choice(colors)) + + # 生成车辆 + npc_vehicle = world.try_spawn_actor(npc_bp, valid_spawn_points[i]) + + if npc_vehicle: + # 启用自动驾驶模式并设置速度限制 + npc_vehicle.set_autopilot(True) + + # 设置NPC车辆的速度限制(让它们能正常行驶) + traffic_manager = client.get_trafficmanager() + traffic_manager.set_global_distance_to_leading_vehicle(2.5) # 设置跟车距离 + traffic_manager.set_random_device_seed(12345) # 设置随机种子确保一致性 + + # 为每个NPC车辆设置不同的速度限制(避免所有车速度相同导致堵塞) + target_speed = random.uniform(30.0, 50.0) # 30-50 km/h + traffic_manager.vehicle_percentage_speed_difference(npc_vehicle, random.uniform(-10, 10)) + + # 设置NPC车辆的驾驶行为(更安全、更智能) + traffic_manager.auto_lane_change(npc_vehicle, True) # 允许自动变道 + traffic_manager.distance_to_leading_vehicle(npc_vehicle, 3.0) # 设置跟车距离 + traffic_manager.collision_detection(npc_vehicle, world, True) # 启用碰撞检测 + + npc_vehicles.append(npc_vehicle) + print(f"生成NPC车辆 {i + 1}: {npc_bp.id} 在位置 {valid_spawn_points[i].location}") + + # 短暂暂停,避免生成时的碰撞 + time.sleep(0.1) + + except Exception as e: + print(f"生成NPC车辆 {i + 1} 时出错: {e}") + +print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") # 配置传感器(简化配置) third_camera_bp = blueprint_library.find('sensor.camera.rgb') @@ -661,7 +653,6 @@ def third_camera_callback(image): third_image = array[:, :, :3] - def front_camera_callback(image): global front_image array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) @@ -694,33 +685,6 @@ def front_camera_callback(image): # 障碍物信息历史 obstacle_history = deque(maxlen=5) - -def front_camera_callback(image): - global front_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("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) - -# 修复6: 初始化控制器 -nn_controller = ImprovedNeuralController() -traditional_controller = TraditionalController(world) - -# 控制变量 -throttle = 0.3 # 更保守的初始油门 -steer = 0.0 -brake = 0.0 -NEURAL_NETWORK_MODE = False # 默认使用传统控制,更稳定 - -# 转向历史,用于平滑 -steer_history = deque(maxlen=10) - - print("初始化车辆状态...") vehicle.set_simulate_physics(True) @@ -733,31 +697,65 @@ def front_camera_callback(image): hand_brake=False )) + +# 添加NPC车辆管理函数 +def check_and_reset_stuck_npcs(): + """检查并重置卡住的NPC车辆""" + for npc in npc_vehicles: + try: + npc_speed = math.sqrt( + npc.get_velocity().x ** 2 + npc.get_velocity().y ** 2 + npc.get_velocity().z ** 2) * 3.6 + # 如果NPC车辆速度过低(小于1km/h)且没有碰撞,可能是卡住了 + if npc_speed < 1.0: + print(f"检测到NPC车辆 {npc.id} 可能卡住,尝试重置...") + # 获取当前位置 + current_transform = npc.get_transform() + # 寻找最近的可用出生点 + closest_point = None + min_distance = float('inf') + for point in spawn_points: + distance = point.location.distance(current_transform.location) + if distance < min_distance and distance > 10.0: # 避免重置到太近的位置 + min_distance = distance + closest_point = point + + if closest_point: + npc.set_transform(closest_point) + npc.set_autopilot(True) + print(f"重置NPC车辆 {npc.id} 到新位置") + except: + pass + + try: print("自动驾驶系统启动 - 初始模式: 传统控制") print("控制键: q-退出, m-切换控制模式, r-重置车辆, t-传统模式, n-神经网络模式") + print(f"当前有 {len(npc_vehicles)} 辆NPC车辆在运行") frame_count = 0 stuck_count = 0 last_position = vehicle.get_location() success_count = 0 # 成功运行计数器 - collision_count = 0 # 碰撞计数器 last_collision_time = 0 # 上次碰撞时间 - + last_npc_check_time = 0 # 上次检查NPC的时间 # 主循环 while True: world.tick() frame_count += 1 + # 定期检查NPC车辆状态(每100帧检查一次) + if frame_count - last_npc_check_time > 100: + check_and_reset_stuck_npcs() + last_npc_check_time = frame_count + # 获取车辆状态 vehicle_transform = vehicle.get_transform() vehicle_location = vehicle.get_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() obstacle_history.append(obstacle_info) @@ -772,11 +770,6 @@ def front_camera_callback(image): f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, " f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}") - - print( - f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}") - - # 修复8: 改进的卡住检测 current_position = vehicle_location distance_moved = current_position.distance(last_position) @@ -789,15 +782,6 @@ def front_camera_callback(image): stuck_count = 0 success_count += 1 # 成功运行一帧 - - last_position = current_position - - # 修复9: 更智能的卡住恢复 - if stuck_count > 15: # 1.5秒后认为卡住 - print("检测到车辆卡住,执行恢复程序...") - - - last_position = current_position # 修复9: 更智能的卡住恢复 @@ -810,7 +794,6 @@ def front_camera_callback(image): )) time.sleep(0.5) - # 检查是否有前方障碍物 if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: print("前方有障碍物,尝试倒车...") @@ -849,37 +832,6 @@ def front_camera_callback(image): # 记录转向历史 steer_history.append(steer) - - - # 然后尝试不同方向的脱困 - recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向 - vehicle.apply_control(carla.VehicleControl( - throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False - )) - time.sleep(1.0) - - stuck_count = 0 - success_count = 0 - - # 每成功运行100帧显示一次状态 - if success_count % 100 == 0: - print(f"已成功运行 {success_count} 帧") - - # 控制逻辑 - if NEURAL_NETWORK_MODE: - # 神经网络控制 - nn_throttle, nn_brake, nn_steer = nn_controller.get_control( - front_image, vehicle_speed, steer_history - ) - - # 修复10: 更激进的控制平滑 - throttle = 0.3 * throttle + 0.7 * nn_throttle - brake = 0.3 * brake + 0.7 * nn_brake - steer = 0.2 * steer + 0.8 * nn_steer - - # 记录转向历史 - steer_history.append(steer) - else: # 传统控制 - 更稳定 throttle, brake, steer = traditional_controller.get_control(vehicle) @@ -900,7 +852,6 @@ def front_camera_callback(image): if third_image is not None: display_image = third_image.copy() - # 可视化障碍物检测结果 display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform) @@ -910,7 +861,6 @@ def front_camera_callback(image): cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else '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) @@ -935,20 +885,6 @@ def front_camera_callback(image): cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image) - 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 stuck_count > 5: - cv2.putText(display_image, "STUCK DETECTED!", (10, 180), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - cv2.imshow('自动驾驶系统 - 修复版', display_image) - - key = cv2.waitKey(1) & 0xFF if key == ord('q'): break @@ -969,15 +905,11 @@ def front_camera_callback(image): brake = 1.0 stuck_count = 0 success_count = 0 - collision_count = 0 steer_history.clear() obstacle_history.clear() print("车辆已重置") - steer_history.clear() - - time.sleep(0.01) except KeyboardInterrupt: @@ -996,7 +928,10 @@ def front_camera_callback(image): # 销毁actor for actor in world.get_actors(): if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): - actor.destroy() + try: + actor.destroy() + except: + pass # 恢复设置 settings.synchronous_mode = False