diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md index 0369bfd2ed..2bc2c004c9 100644 --- a/src/car_navigation_system/README.md +++ b/src/car_navigation_system/README.md @@ -5,17 +5,22 @@ ## 核心功能 - **多模态感知系统**：集成 RGB 摄像头（前视 + 第三视角）与障碍物检测器，全面获取环境信息 -- **智能避障算法**：基于改进神经网络控制器与传统控制器双模式，动态检测并规避前方障碍物 +- **智能避障算法**：基于改进神经网络控制器与传统控制器双模式，动态检测并规避前方障碍 - **实时可视化**：实时显示第三视角画面，叠加障碍物检测结果、车速、控制状态等关键信息 - **双模式控制**：支持神经网络模式与传统控制模式切换，可根据环境选择最优控制策略 - **智能恢复机制**：内置车辆卡住检测与自动恢复系统，保障行驶稳定性 - **深度神经网络**：结合神经网络与传统控制算法，实现车辆自主行驶与障碍物规避功能 +- **夜晚模式**：支持昼夜切换，自动开启/关闭近光灯 +- **车辆款式切换**：支持多种车辆模型切换 +- **轨迹导航显示**：实时显示道路拓扑地图与车辆行驶轨迹 ## 项目结构 ``` car_navigation_system/ ├── README.md # 项目说明文档 -└── main.py # 主程序文件 +├── main.py # 主程序文件 +├── screenshots/ # 截图保存目录 +└── sync_main.bat # 同步主分支脚本 ``` ## 环境配置 @@ -29,7 +34,7 @@ car_navigation_system/ - 从 [CARLA 官网](https://carla.org/) 下载并安装 CARLA 3.11 - 或使用项目提供的 CARLA 安装包 -2. **安装 Python 依赖包** +2. **安装 Python 依赖** ```bash pip install carla numpy opencv-python matplotlib torch ``` @@ -63,23 +68,29 @@ car_navigation_system/ | q | 退出系统 | | r | 重置车辆位置 | | s | 紧急停止 | -| w | 手动加速（提高油门） | -| a | 手动左转向 | -| d | 手动右转向 | +| x | 切换倒车/前进模式 | +| v | 切换视角（第一人称/第三人称/鸟瞰图） | +| m | 切换地图 | +| w | 切换天气 | +| c | 切换车辆颜色 | +| p | 保存当前画面截图 | +| l | 切换夜晚模式（自动打开/关闭近光灯） | +| u | 切换车辆款式 | +| d | 切换导航轨迹显示 | ## 系统架构 ### 1. 环境初始化模块 - 连接 CARLA 服务器（默认 localhost:2000） -- 加载 Town01 地图，设置异步模式确保连接稳定 -- 配置天气参数（云量30%、无降水、太阳高度角70°） +- 加载地图，设置异步模式确保连接稳定 +- 配置天气参数（云量、降水、太阳高度角） ### 2. 智能体生成模块 -- **主车辆**：特斯拉 Model3（红色），关闭自动驾驶，由自定义控制算法控制 -- **NPC车辆**：随机生成2辆不同类型车辆，开启自动驾驶 +- **主车辆**：特斯拉 Model3，关闭自动驾驶，由自定义控制算法控制 +- **NPC车辆**：随机生成多辆不同类型车辆，开启自动驾驶 ### 3. 传感器系统 -- **第三视角摄像头**：90°视野，分辨率640×480，用于可视化监控 +- **多视角摄像头**：支持第一人称、第三人称、鸟瞰视角 - **图像数据处理**：实时转换和处理相机图像数据 ### 4. 控制系统 @@ -88,7 +99,7 @@ car_navigation_system/ - **转向控制**：基于路点计算最优转向角度 ### 5. 可视化与监控 -- 实时显示第三视角画面 +- 实时显示摄像头画面 - 叠加车速、油门、转向值等状态信息 - 每100帧显示一次运行状态 @@ -97,6 +108,23 @@ car_navigation_system/ - 车辆生成失败时自动清理并重新尝试 - 异常情况时优雅退出并清理资源 +### 7. 夜晚模式系统 +- 一键切换昼夜模式 +- 自动调整天气参数（云层、雾密度、太阳高度角） +- 自动控制近光灯开关 + +### 8. 车辆款式系统 +- 支持多种车辆模型切换（特斯拉Model3、雪佛兰Impala、福特Mustang、奔驰Coupe、日产Patrol） +- 切换时保持车辆颜色和位置 +- 自动重建相机和控制器 + +### 9. 轨迹导航系统 +- 实时生成道路拓扑地图 +- 显示车辆行驶轨迹（黄色线条） +- 在画面右上角显示200x200小地图 +- 红色点标记当前车辆位置 +- 支持最多500个轨迹点记录 + ## 技术特点 - **模块化设计**：清晰的类结构和功能划分 - **鲁棒性强**：多重重试和错误处理机制 @@ -104,12 +132,19 @@ car_navigation_system/ - **易于扩展**：预留了神经网络控制器接口 - **用户友好**：简洁的操作界面和状态显示 +## 截图功能 +按 `p` 键保存当前画面截图，自动命名格式： +``` +screenshot_时间戳_地图名_天气_颜色.png +``` +示例：`screenshot_20260508_204930_Town01_Clear_Red.png` + ## 常见问题 ### 1. 连接 CARLA 服务器失败 - 确保 CARLA 模拟器正在运行 - 检查端口是否为 2000 -- 验证 Town01 地图是否可用 +- 验证地图是否可用 ### 2. 车辆生成失败 - 可能是出生点被占用 @@ -147,6 +182,22 @@ car_navigation_system/ ## 更新日志 +### v1.2.0 +- 添加夜晚模式（按L键切换），自动开启/关闭近光灯 +- 添加车辆款式切换功能（按U键切换），支持5种车辆模型 +- 添加轨迹导航显示功能（按D键切换） +- 实现道路拓扑地图生成 +- 在画面右上角显示小地图和行驶轨迹 +- 优化车辆切换时的稳定性和错误处理 + +### v1.1.0 +- 添加多视角切换功能（第一人称/第三人称/鸟瞰图） +- 添加地图切换功能 +- 添加天气切换功能 +- 添加车辆颜色切换功能 +- 实现截图功能（按p键保存） +- 优化倒车控制功能 + ### v1.0.0 - 初始化项目 - 实现基本的自动驾驶功能 @@ -154,4 +205,4 @@ car_navigation_system/ - 实现路点跟踪控制算法 - 添加NPC车辆生成 - 实现车辆重置功能 -- 添加紧急停止功能 \ No newline at end of file +- 添加紧急停止功能 diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 716ab6a423..2dc3557ee7 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -101,10 +101,12 @@ def __init__(self): self.client = None self.world = None self.vehicle = None + self.vehicles = [] # 存储所有车辆（包括主车和NPC） + self.controllers = [] # 存储每个车辆的控制器 + self.current_vehicle_index = 0 # 当前关注的车辆索引 self.cameras = {} # 存储多个相机 self.controller = None self.camera_image = None - self.current_view = 'third_person' # 当前视角模式：'first_person', 'third_person', 'birdseye' def connect(self): """连接到CARLA服务器""" @@ -274,11 +276,81 @@ def get_view_name(self): } return view_names.get(self.current_view, 'Unknown') + def setup_all_vehicles_cameras(self): + """为所有车辆设置相机系统""" + print("正在为所有车辆设置相机...") + + try: + blueprint_library = self.world.get_blueprint_library() + camera_bp = blueprint_library.find('sensor.camera.rgb') + + # 设置相机属性 + camera_bp.set_attribute('image_size_x', '640') + camera_bp.set_attribute('image_size_y', '480') + camera_bp.set_attribute('fov', '90') + + # 为每辆车创建相机 + for i, vehicle in enumerate(self.vehicles): + if vehicle is None: + continue + + print(f"为车辆 {i} 设置相机...") + + # 第三人称相机 - 用于跟随视角 + third_person_transform = carla.Transform( + carla.Location(x=-8.0, z=6.0), # 在车辆后方上方 + carla.Rotation(pitch=-20.0) # 向下看 + ) + third_person_camera = self.world.spawn_actor( + camera_bp, third_person_transform, attach_to=vehicle + ) + third_person_camera.listen(lambda image, idx=i: self.camera_callback(image, idx, 'third_person')) + self.cameras[f'vehicle_{i}_third_person'] = third_person_camera + + print(f"已为 {len(self.vehicles)} 辆车设置相机") + return True + + except Exception as e: + print(f"设置多车辆相机时出错: {e}") + return False + + def camera_callback(self, image, vehicle_index, view_mode=None): + """相机数据回调""" + try: + # 只有当前关注车辆的相机数据才会被使用 + if vehicle_index == self.current_vehicle_index and view_mode == 'third_person': + # 转换图像数据 + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + self.camera_image = array[:, :, :3] # RGB通道 + except: + pass + + def update_current_vehicle_view(self): + """更新当前关注的车辆视角""" + vehicle_label = "主车辆（红色特斯拉）" if self.current_vehicle_index == 0 else f"NPC车辆 {self.current_vehicle_index}" + print(f"已切换到: {vehicle_label} 视角") + def setup_controller(self): """设置控制器""" self.controller = SimpleController(self.world, self.vehicle) print("控制器设置完成") + def setup_all_controllers(self): + """为所有车辆设置控制器""" + print("正在为所有车辆设置控制器...") + self.controllers = [] + + for i, vehicle in enumerate(self.vehicles): + controller = SimpleController(self.world, vehicle) + self.controllers.append(controller) + vehicle_name = "主车辆" if i == 0 else f"NPC车辆{i}" + print(f" {vehicle_name} 控制器设置完成") + + # 确保self.controller指向主车辆控制器 + self.controller = self.controllers[0] + print("所有控制器设置完成") + def run(self): """主运行循环""" print("\n" + "=" * 50) @@ -293,11 +365,6 @@ def run(self): if not self.spawn_vehicle(): return - # 设置相机 - if not self.setup_camera(): - # 即使相机失败也继续运行 - print("警告：相机设置失败，继续运行...") - # 设置控制器 self.setup_controller() @@ -313,72 +380,104 @@ def run(self): ) self.world.set_weather(weather) - # 生成一些NPC车辆 - self.spawn_npc_vehicles(2) + # 生成2辆NPC车辆（加上主车辆共3辆特斯拉） + npc_vehicles = self.spawn_npc_vehicles(2) + + # 将所有车辆添加到列表中（主车辆在前，NPC在后） + self.vehicles = [self.vehicle] + npc_vehicles + + # 为所有车辆设置控制器 + self.setup_all_controllers() print("\n系统准备就绪！") + print(f"共 {len(self.vehicles)} 辆车可用") print("控制指令:") print(" q - 退出程序") - print(" r - 重置车辆") + print(" r - 重置当前车辆") print(" s - 紧急停止") print(" x - 切换倒车/前进模式（速度为0时生效）") - print(" v - 切换视角（第一人称/第三人称/鸟瞰图）") + for i in range(len(self.vehicles)): + if i == 0: + print(f" 1 - 切换到主车辆视角（红色特斯拉）") + else: + print(f" {i+1} - 切换到NPC车辆{i}视角") print("\n开始自动驾驶...\n") + # 为所有车辆创建相机 + self.setup_all_vehicles_cameras() + frame_count = 0 running = True + + # 卡住检测变量 + stuck_frame_count = 0 + stuck_threshold = 500 # 连续500帧速度低于2km/h认为卡住 try: while running: - # 获取车辆状态 - velocity = self.vehicle.get_velocity() + # 获取当前关注车辆的状态 + current_vehicle = self.vehicles[self.current_vehicle_index] + velocity = current_vehicle.get_velocity() speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 - # 获取控制指令（现在返回4个值，原代码返回3个值） - # throttle, brake, steer = self.controller.get_control() # 原代码 - throttle, brake, steer, reverse = self.controller.get_control() # 新代码 + # 检测主车辆是否卡住 + if self.current_vehicle_index == 0: + if speed < 2.0: # 速度低于2km/h + stuck_frame_count += 1 + if stuck_frame_count >= stuck_threshold: + print("检测到主车辆卡住，自动重置...") + self.reset_vehicle() + stuck_frame_count = 0 + else: + stuck_frame_count = 0 - # 应用控制 + # 对所有车辆应用控制器 + # 主车辆（索引0）使用自定义控制器 + controller = self.controllers[0] + throttle, brake, steer, reverse = controller.get_control() control = carla.VehicleControl( throttle=float(throttle), brake=float(brake), steer=float(steer), hand_brake=False, - # reverse=False # 原代码 - reverse=reverse # 新代码，支持倒车 + reverse=reverse ) self.vehicle.apply_control(control) + # NPC车辆使用内置自动驾驶（自动避开障碍物） + for i in range(1, len(self.vehicles)): + self.vehicles[i].set_autopilot(True, 16) # 16 = ARLA_AUTOPILIT_IGNORE_ALL + # 更新显示 if self.camera_image is not None: display_img = self.camera_image.copy() - # 添加状态信息 + # 显示当前车辆编号（绿色） + cv2.putText(display_img, f"Vehicle: {self.current_vehicle_index + 1}", + (20, 30), cv2.FONT_HERSHEY_SIMPLEX, + 0.6, (0, 255, 0), 2) + + # 添加状态信息（显示当前关注车辆的数据） cv2.putText(display_img, f"Speed: {speed:.1f} km/h", - (20, 40), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (255, 255, 255), 2) + (20, 60), cv2.FONT_HERSHEY_SIMPLEX, + 0.6, (255, 255, 255), 2) cv2.putText(display_img, f"Throttle: {throttle:.2f}", - (20, 80), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (255, 255, 255), 2) + (20, 90), cv2.FONT_HERSHEY_SIMPLEX, + 0.6, (255, 255, 255), 2) cv2.putText(display_img, f"Steer: {steer:.2f}", (20, 120), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (255, 255, 255), 2) + 0.6, (255, 255, 255), 2) cv2.putText(display_img, f"Frame: {frame_count}", - (20, 160), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (255, 255, 255), 2) - - # 显示倒车状态（新功能） - if self.controller.manual_reverse: - cv2.putText(display_img, "REVERSE MODE", - (20, 200), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (0, 0, 255), 2) # 红色显示 - - # 显示当前视角模式 - cv2.putText(display_img, f"View: {self.get_view_name()}", - (20, 240), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (0, 255, 0), 2) # 绿色显示 - - cv2.imshow('Autonomous Driving - Simple Version', display_img) + (20, 150), cv2.FONT_HERSHEY_SIMPLEX, + 0.6, (255, 255, 255), 2) + + # 显示倒车状态 + if self.current_vehicle_index == 0 and self.controller.manual_reverse: + cv2.putText(display_img, "REVERSE MODE", + (20, 240), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (0, 0, 255), 2) + + cv2.imshow('Autonomous Driving - Multi-Vehicle View', display_img) # 处理按键 key = cv2.waitKey(1) & 0xFF @@ -386,26 +485,30 @@ def run(self): print("正在退出...") running = False elif key == ord('r'): - self.reset_vehicle() + self.reset_current_vehicle() elif key == ord('s'): - # 紧急停止 - self.vehicle.apply_control(carla.VehicleControl( - throttle=0.0, brake=1.0, hand_brake=True - )) + # 紧急停止（只对主车辆生效） + if self.current_vehicle_index == 0: + self.vehicle.apply_control(carla.VehicleControl( + throttle=0.0, brake=1.0, hand_brake=True + )) print("紧急停止") elif key == ord('x'): # 切换倒车模式（只在速度接近0时允许切换） - if speed < 1.0: # 速度小于1km/h时允许切换 + if self.current_vehicle_index == 0 and speed < 1.0: self.controller.toggle_reverse() + elif self.current_vehicle_index != 0: + print("只有主车辆可以切换倒车模式") else: print("请先减速到接近停止（速度<1km/h）再切换倒车模式") - elif key == ord('v'): - # 切换视角模式 - view_modes = ['third_person', 'first_person', 'birdseye'] - current_index = view_modes.index(self.current_view) - next_index = (current_index + 1) % len(view_modes) - self.current_view = view_modes[next_index] - self.update_camera_view() + elif ord('1') <= key <= ord('9'): + # 动态切换车辆视角（按数字键1-9） + vehicle_index = key - ord('1') + if vehicle_index < len(self.vehicles): + self.current_vehicle_index = vehicle_index + self.update_current_vehicle_view() + else: + print(f"车辆 {vehicle_index + 1} 不存在") frame_count += 1 @@ -423,40 +526,65 @@ def run(self): self.cleanup() def spawn_npc_vehicles(self, count=2): - """生成NPC车辆（简化）""" + """生成NPC车辆（改进版：确保生成在合适位置）""" print(f"正在生成 {count} 辆NPC车辆...") try: blueprint_library = self.world.get_blueprint_library() spawn_points = self.world.get_map().get_spawn_points() + # 获取主车辆当前位置 + main_location = self.vehicle.get_location() + + # 筛选出距离主车辆较远的出生点（至少80米） + far_spawn_points = [ + sp for sp in spawn_points + if sp.location.distance(main_location) > 80.0 + ] + + if not far_spawn_points: + far_spawn_points = spawn_points + npc_vehicles = [] - for i in range(min(count, len(spawn_points))): - # 跳过主车辆的出生点 - if i == 0: - continue + # 生成指定数量的特斯拉车辆 + for i in range(count): + # 使用不同的出生点 + spawn_index = i % len(far_spawn_points) try: - # 随机选择车辆类型 - vehicle_bps = list(blueprint_library.filter('vehicle.*')) - if vehicle_bps: - vehicle_bp = random.choice(vehicle_bps) - - # 生成NPC - npc = self.world.try_spawn_actor(vehicle_bp, spawn_points[i]) - - if npc: - npc.set_autopilot(True) - npc_vehicles.append(npc) - print(f"生成NPC车辆 {len(npc_vehicles)}") - except: + # 使用特斯拉Model3蓝图 + vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + if not vehicle_bp: + print("未找到特斯拉蓝图，尝试其他车辆...") + vehicle_bp = blueprint_library.filter('vehicle.*')[0] + + # 设置不同颜色区分 + colors = [[0, 255, 0], [0, 0, 255], [255, 255, 0], [255, 0, 255], [0, 255, 255]] + color = colors[i % len(colors)] + vehicle_bp.set_attribute('color', f'{color[0]},{color[1]},{color[2]}') + + # 生成NPC + npc = self.world.try_spawn_actor(vehicle_bp, far_spawn_points[spawn_index]) + + if npc: + # 立即开启自动驾驶，禁用交通灯检测 + npc.set_autopilot(True, 16) + npc_vehicles.append(npc) + print(f"生成NPC车辆 {len(npc_vehicles)} (特斯拉Model3)") + print(f" 位置: {far_spawn_points[spawn_index].location}") + else: + print(f"无法在出生点 {spawn_index} 生成NPC车辆") + except Exception as e: + print(f"生成NPC车辆 {i+1} 时出错: {e}") pass print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") + return npc_vehicles except Exception as e: print(f"生成NPC车辆时出错: {e}") + return [] def reset_vehicle(self): """重置车辆位置""" @@ -464,10 +592,38 @@ def reset_vehicle(self): spawn_points = self.world.get_map().get_spawn_points() if spawn_points: - new_spawn_point = random.choice(spawn_points) + # 选择一个距离当前车辆较远的出生点，避免重置后立即撞到 + current_location = self.vehicle.get_location() + far_spawn_points = [ + sp for sp in spawn_points + if sp.location.distance(current_location) > 50.0 # 至少50米远 + ] + if far_spawn_points: + new_spawn_point = random.choice(far_spawn_points) + else: + new_spawn_point = random.choice(spawn_points) + self.vehicle.set_transform(new_spawn_point) print(f"车辆已重置到新位置: {new_spawn_point.location}") + # 重置控制器状态 + self.controllers[0].last_waypoint = None + + # 等待重置完成 + time.sleep(0.5) + + def reset_current_vehicle(self): + """重置当前关注的车辆位置""" + current_vehicle = self.vehicles[self.current_vehicle_index] + vehicle_label = "主车辆" if self.current_vehicle_index == 0 else f"NPC车辆 {self.current_vehicle_index}" + print(f"重置{vehicle_label}位置...") + + spawn_points = self.world.get_map().get_spawn_points() + if spawn_points: + new_spawn_point = random.choice(spawn_points) + current_vehicle.set_transform(new_spawn_point) + print(f"{vehicle_label}已重置到新位置: {new_spawn_point.location}") + # 等待重置完成 time.sleep(0.5)