From 5ac34023519ae2e155fc26b0a9ab7bfd2200a1a4 Mon Sep 17 00:00:00 2001 From: XieTJ <2922472198@qq.com> Date: Tue, 12 May 2026 22:13:43 +0800 Subject: [PATCH] Add files via upload --- src/car_navigation_system/main.py | 317 +++++++++--------------------- 1 file changed, 96 insertions(+), 221 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 242419ed34..4bee55530a 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -1,4 +1,4 @@ -# -------------------------- +# -------------------------- # 简化修复版：确保车辆正确生成 # -------------------------- @@ -129,20 +129,22 @@ def __init__(self): ] # 车辆颜色列表 self.current_color_index = 0 # 当前颜色索引 self.screenshot_dir = 'screenshots' # 截图保存目录 - self.is_night_mode = False # 夜晚模式标志 - self.vehicle_models = [ # 车辆款式列表 - 'vehicle.tesla.model3', # 特斯拉 Model3 - 'vehicle.chevrolet.impala', # 雪佛兰 Impala - 'vehicle.ford.mustang', # 福特 Mustang - 'vehicle.mercedes.coupe', # 奔驰 Coupe - 'vehicle.nissan.patrol', # 日产 Patrol - ] # 车辆款式列表 - self.current_model_index = 0 # 当前车辆款式索引 - self.show_trajectory = False # 轨迹显示标志 - self.trajectory_points = deque(maxlen=500) # 轨迹点集合 - self.map_img = None # 存储地图图像 - self.map_width = 800 # 地图图像宽度 - self.map_height = 800 # 地图图像高度 + + # 车辆品牌列表（经过验证可用的蓝图） + self.vehicle_models = [ + ('vehicle.tesla.model3', 'Tesla Model3'), + ('vehicle.ford.mustang', 'Ford Mustang'), + ('vehicle.audi.tt', 'Audi TT'), + ('vehicle.mercedes.coupe', 'Mercedes Coupe'), + ('vehicle.jeep.wrangler_rubicon', 'Jeep Wrangler Rubicon'), + ('vehicle.nissan.patrol', 'Nissan Patrol'), + ('vehicle.audi.etron', 'Audi e-tron'), + ('vehicle.lincoln.mkz_2020', 'Lincoln MKZ 2020'), + ('vehicle.chevrolet.impala', 'Chevrolet Impala'), + ('vehicle.bmw.grandtourer', 'BMW Grand Tourer'), + ] + self.current_vehicle_index = 0 # 当前车辆品牌索引 + self.spawn_point = None # 存储车辆出生点 def connect(self): """连接到CARLA服务器""" @@ -179,20 +181,25 @@ def connect(self): return False def spawn_vehicle(self): - """生成车辆 - 简化版本""" + """生成车辆 - 使用当前选中的车辆品牌""" print("正在生成车辆...") try: # 获取蓝图库 blueprint_library = self.world.get_blueprint_library() - # 选择车辆蓝图 - vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + # 获取当前选中的车辆品牌 + vehicle_bp_name, vehicle_display_name = self.vehicle_models[self.current_vehicle_index] + vehicle_bp = blueprint_library.find(vehicle_bp_name) + if not vehicle_bp: - print("未找到特斯拉蓝图，尝试其他车辆...") + print(f"未找到 {vehicle_display_name} 蓝图，尝试其他车辆...") vehicle_bp = blueprint_library.filter('vehicle.*')[0] + vehicle_display_name = "Default Vehicle" - vehicle_bp.set_attribute('color', '255,0,0') # 红色 + # 设置车辆颜色 + color = self.car_colors[self.current_color_index] + vehicle_bp.set_attribute('color', f'{color[0]},{color[1]},{color[2]}') # 获取出生点 spawn_points = self.world.get_map().get_spawn_points() @@ -204,6 +211,7 @@ def spawn_vehicle(self): # 选择第一个出生点 spawn_point = spawn_points[0] + self.spawn_point = spawn_point # 保存出生点 # 尝试生成车辆 self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) @@ -220,6 +228,7 @@ def spawn_vehicle(self): if self.vehicle: print(f"车辆生成成功！ID: {self.vehicle.id}") + print(f"车辆型号: {vehicle_display_name}") print(f"位置: {spawn_point.location}") # 禁用自动驾驶 @@ -303,58 +312,6 @@ def update_camera_view(self): """更新相机视角""" print(f"已切换到{self.get_view_name()}视角") - def generate_topology_map(self): - """生成道路拓扑地图图像""" - try: - # 创建黑色背景 - self.map_img = np.zeros((self.map_width, self.map_height, 3), dtype=np.uint8) - - # 获取地图拓扑 - map = self.world.get_map() - topology = map.get_topology() - - # 计算道路节点的范围 - all_x = [] - all_y = [] - for waypoint in map.generate_waypoints(2.0): - all_x.append(waypoint.transform.location.x) - all_y.append(waypoint.transform.location.y) - - if not all_x or not all_y: - print("无法获取道路拓扑信息") - return - - min_x, max_x = min(all_x), max(all_x) - min_y, max_y = min(all_y), max(all_y) - - # 坐标缩放比例 - scale_x = (self.map_width - 100) / (max_x - min_x) if max_x != min_x else 1 - scale_y = (self.map_height - 100) / (max_y - min_y) if max_y != min_y else 1 - self.map_scale = min(scale_x, scale_y) * 0.8 - self.map_offset_x = min_x - self.map_offset_y = min_y - - # 绘制道路 - for waypoint in map.generate_waypoints(2.0): - wp_x = int((waypoint.transform.location.x - self.map_offset_x) * self.map_scale + 50) - wp_y = int((waypoint.transform.location.y - self.map_offset_y) * self.map_scale + 50) - - # 绘制道路点（灰色） - cv2.circle(self.map_img, (wp_x, wp_y), 1, (80, 80, 80), -1) - - print("道路拓扑地图生成成功") - except Exception as e: - print(f"生成拓扑地图时出错: {e}") - - def world_to_map(self, location): - """将世界坐标转换为地图图像坐标""" - try: - x = int((location.x - self.map_offset_x) * self.map_scale + 50) - y = int((location.y - self.map_offset_y) * self.map_scale + 50) - return (x, y) - except: - return (400, 400) - def switch_map(self): """切换到下一个地图""" try: @@ -413,10 +370,7 @@ def switch_map(self): # 应用当前天气 self.set_weather(self.current_weather) - - # 重新生成道路拓扑地图 - self.generate_topology_map() - + print(f"地图切换成功: {self.current_map}") except Exception as e: @@ -465,40 +419,6 @@ def switch_weather(self): except Exception as e: print(f"切换天气时出错: {e}") - def toggle_night_mode(self): - """切换夜晚模式并自动打开/关闭近光灯""" - try: - self.is_night_mode = not self.is_night_mode - - if self.is_night_mode: - # 切换到夜晚模式 - night_weather = carla.WeatherParameters( - cloudiness=80.0, - precipitation=0.0, - sun_altitude_angle=-30.0, # 负角度表示夜晚 - fog_density=30.0, - fog_distance=50.0 - ) - self.world.set_weather(night_weather) - - # 打开近光灯 - if self.vehicle: - self.vehicle.set_light_state(carla.VehicleLightState(carla.VehicleLightState.LowBeam)) - - print("已切换到夜晚模式，近光灯已打开") - else: - # 切换回白天模式（使用当前天气） - self.set_weather(self.current_weather) - - # 关闭近光灯 - if self.vehicle: - self.vehicle.set_light_state(carla.VehicleLightState(carla.VehicleLightState.NONE)) - - print("已切换到白天模式，近光灯已关闭") - - except Exception as e: - print(f"切换夜晚模式时出错: {e}") - def switch_color(self): """切换车辆颜色""" try: @@ -528,11 +448,13 @@ def switch_color(self): self.vehicle.destroy() self.vehicle = None - # 创建新车辆蓝图 + # 创建新车辆蓝图，使用当前选中的品牌 blueprint_library = self.world.get_blueprint_library() - vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + vehicle_bp_name, vehicle_display_name = self.vehicle_models[self.current_vehicle_index] + vehicle_bp = blueprint_library.find(vehicle_bp_name) if not vehicle_bp: vehicle_bp = blueprint_library.filter('vehicle.*')[0] + vehicle_display_name = "Default Vehicle" # 设置新颜色 vehicle_bp.set_attribute('color', f'{color[0]},{color[1]},{color[2]}') @@ -576,13 +498,30 @@ def switch_color(self): if not self.vehicle: self.spawn_vehicle() - def switch_vehicle_model(self): - """切换车辆款式""" + def switch_vehicle(self): + """切换车辆品牌 - 显示菜单供选择""" try: + # 显示车辆品牌菜单 + print("\n" + "=" * 40) + print("选择车辆品牌:") + print("=" * 40) + for i, (bp_name, display_name) in enumerate(self.vehicle_models): + marker = " >>> " if i == self.current_vehicle_index else " " + print(f"{marker}[{i + 1}] {display_name}") + print("=" * 40) + print("按 1-9 选择车辆，q 取消") + print("=" * 40) + + # 获取用户输入（在实际运行中，这需要通过输入函数实现） + # 这里我们直接切换到下一个车辆 + self.current_vehicle_index = (self.current_vehicle_index + 1) % len(self.vehicle_models) + _, new_vehicle_name = self.vehicle_models[self.current_vehicle_index] + if self.vehicle: + # 获取当前车辆位置和方向 transform = self.vehicle.get_transform() - # 停止所有相机 + # 停止相机 for view_mode, camera in self.cameras.items(): if camera: try: @@ -596,61 +535,54 @@ def switch_vehicle_model(self): self.vehicle.destroy() self.vehicle = None - # 切换到下一个车辆款式 - self.current_model_index = (self.current_model_index + 1) % len(self.vehicle_models) - next_model = self.vehicle_models[self.current_model_index] - - # 获取蓝图库 + # 创建新车辆蓝图 blueprint_library = self.world.get_blueprint_library() - vehicle_bp = blueprint_library.find(next_model) - - # 如果找不到指定车型，随机选择一个 + vehicle_bp = blueprint_library.find(self.vehicle_models[self.current_vehicle_index][0]) if not vehicle_bp: - all_vehicles = blueprint_library.filter('vehicle.*') - if all_vehicles: - vehicle_bp = random.choice(list(all_vehicles)) - print(f"未找到 {next_model}，使用随机车辆: {vehicle_bp.id}") + vehicle_bp = blueprint_library.filter('vehicle.*')[0] + new_vehicle_name = "Default Vehicle" + + # 设置当前颜色 + color = self.car_colors[self.current_color_index] + vehicle_bp.set_attribute('color', f'{color[0]},{color[1]},{color[2]}') + + # 首先尝试在相同位置生成新车辆 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, transform) + + # 如果失败，尝试使用出生点 + if not self.vehicle: + if self.spawn_point: + self.vehicle = self.world.try_spawn_actor(vehicle_bp, self.spawn_point) + + if not self.vehicle: + spawn_points = self.world.get_map().get_spawn_points() + for spawn_point in spawn_points[:5]: + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if self.vehicle: + print("车辆已移动到新位置") + break - if vehicle_bp: - # 设置当前颜色 - color = self.car_colors[self.current_color_index] - vehicle_bp.set_attribute('color', f'{color[0]},{color[1]},{color[2]}') + if self.vehicle: + # 禁用自动驾驶 + self.vehicle.set_autopilot(False) - # 尝试在相同位置生成新车辆 - self.vehicle = self.world.try_spawn_actor(vehicle_bp, transform) + # 重新设置相机 + self.setup_camera() - # 如果失败，尝试使用出生点 - if not self.vehicle: - spawn_points = self.world.get_map().get_spawn_points() - for spawn_point in spawn_points[:5]: - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) - if self.vehicle: - print("车辆已移动到新位置") - break + # 重新设置控制器 + self.setup_controller() - if self.vehicle: - # 禁用自动驾驶 - self.vehicle.set_autopilot(False) - - # 重新设置相机 - self.setup_camera() - - # 重新设置控制器 - self.setup_controller() - - print(f"车辆款式已切换: {next_model.split('.')[-1]}") - else: - print("无法生成新车辆，款式切换失败") - self.current_model_index = (self.current_model_index - 1) % len(self.vehicle_models) - self.spawn_vehicle() + print(f"\n车辆品牌已切换: {new_vehicle_name}") else: - print("无法获取车辆蓝图") + print("无法生成新车辆，品牌切换失败") + # 尝试恢复车辆 self.spawn_vehicle() else: - print("车辆不存在，无法切换款式") + print("车辆不存在，无法切换品牌") + except Exception as e: - print(f"切换车辆款式时出错: {e}") - self.current_model_index = (self.current_model_index - 1) % len(self.vehicle_models) + print(f"切换车辆品牌时出错: {e}") + # 尝试恢复车辆 if not self.vehicle: self.spawn_vehicle() @@ -739,9 +671,6 @@ def run(self): # 生成一些NPC车辆 self.spawn_npc_vehicles(2) - # 生成道路拓扑地图 - self.generate_topology_map() - print("\n系统准备就绪！") print("控制指令:") print(" q - 退出程序") @@ -752,10 +681,8 @@ def run(self): print(" m - 切换地图（Town01/Town02/Town03等）") print(" w - 切换天气（晴天/雨天/多云/湿滑）") print(" c - 切换车辆颜色") + print(" b - 切换车辆品牌（Tesla/Ford/Mustang等）") print(" p - 保存当前画面截图") - print(" l - 切换夜晚模式（自动打开/关闭近光灯）") - print(" u - 切换车辆款式") - print(" d - 切换导航轨迹显示") print("\n开始自动驾驶...\n") frame_count = 0 @@ -782,11 +709,6 @@ def run(self): ) self.vehicle.apply_control(control) - # 记录轨迹点 - if self.show_trajectory: - vehicle_location = self.vehicle.get_location() - self.trajectory_points.append(vehicle_location) - # 更新显示 if self.camera_image is not None: display_img = self.camera_image.copy() @@ -832,43 +754,6 @@ def run(self): cv2.putText(display_img, f"Color: {current_color_name}", (20, 360), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 128, 255), 2) # 橙色显示 - - # 显示当前车辆款式 - current_model = self.vehicle_models[self.current_model_index] - model_name = current_model.split('.')[-1] - cv2.putText(display_img, f"Model: {model_name}", - (20, 400), cv2.FONT_HERSHEY_SIMPLEX, - 0.8, (128, 255, 0), 2) # 亮绿色显示 - - # 绘制道路轨迹 - if self.show_trajectory and self.map_img is not None: - # 创建地图副本 - map_display = self.map_img.copy() - - # 绘制轨迹 - if len(self.trajectory_points) > 1: - for i in range(1, len(self.trajectory_points)): - pt1 = self.world_to_map(self.trajectory_points[i-1]) - pt2 = self.world_to_map(self.trajectory_points[i]) - cv2.line(map_display, pt1, pt2, (0, 255, 255), 2) - - # 绘制车辆位置（红色点） - vehicle_loc = self.vehicle.get_location() - vehicle_pt = self.world_to_map(vehicle_loc) - cv2.circle(map_display, vehicle_pt, 5, (0, 0, 255), -1) - - # 在主显示图像右上角显示地图 - map_resized = cv2.resize(map_display, (200, 200)) - display_img[10:210, display_img.shape[1]-210:display_img.shape[1]-10] = map_resized - - # 显示轨迹状态 - cv2.putText(display_img, f"Trajectory: ON ({len(self.trajectory_points)} pts)", - (20, 440), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (0, 255, 255), 2) - else: - cv2.putText(display_img, "Trajectory: OFF", - (20, 440), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (128, 128, 128), 2) cv2.imshow('Autonomous Driving - Simple Version', display_img) @@ -907,25 +792,15 @@ def run(self): elif key == ord('c'): # 切换车辆颜色 self.switch_color() + elif key == ord('b'): + # 切换车辆品牌 + self.switch_vehicle() elif key == ord('p'): # 保存截图 if self.camera_image is not None: self.take_screenshot(self.camera_image) else: print("当前没有图像可保存") - elif key == ord('l') or key == ord('L'): - # 切换夜晚模式（支持大小写） - self.toggle_night_mode() - elif key == ord('u') or key == ord('U'): - # 切换车辆款式（支持大小写） - self.switch_vehicle_model() - elif key == ord('d') or key == ord('D'): - # 切换轨迹显示（支持大小写） - self.show_trajectory = not self.show_trajectory - if self.show_trajectory: - print("轨迹显示已开启") - else: - print("轨迹显示已关闭") frame_count += 1