From 6c964b08f9822d6465f9e1d771439744cddea84c Mon Sep 17 00:00:00 2001 From: Yancheng021 <2710932546@qq.com> Date: Tue, 5 May 2026 23:55:24 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=A4=9A=E8=BD=A6?= =?UTF-8?q?=E8=BE=86=E8=A7=86=E8=A7=92=E5=88=87=E6=8D=A2=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 按1/2/3键切换不同车辆视角 --- src/car_navigation_system/main.py | 298 +++++++++++++++++++++++------- 1 file changed, 227 insertions(+), 71 deletions(-) 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) From 31bd3b1bb20aed8f54cc64bc18a5d06a166d208e Mon Sep 17 00:00:00 2001 From: Yancheng021 <2710932546@qq.com> Date: Mon, 8 Jun 2026 22:59:56 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E6=96=B0=E5=A2=9E=20HUD=20=E4=BB=AA?= =?UTF-8?q?=E8=A1=A8=E7=9B=98=E5=8A=9F=E8=83=BD=20-=20=E6=94=AF=E6=8C=81?= =?UTF-8?q?=E5=8F=B3=E4=B8=8A=E8=A7=92=E6=98=BE=E7=A4=BA=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E8=A1=A8=E3=80=81=E6=B2=B9=E9=97=A8=E3=80=81=E6=96=B9=E5=90=91?= =?UTF-8?q?=E7=9B=98=E7=AD=89=E4=BF=A1=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_navigation_system/main.py | 966 +++++++++++++++++++++++------- 1 file changed, 739 insertions(+), 227 deletions(-) diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index 2dc3557ee7..45c62ea179 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -101,12 +101,176 @@ 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' + self.current_map = 'Town01' # 当前地图 + self.available_maps = ['Town01', 'Town02', 'Town03', 'Town04', 'Town05', 'Town06', 'Town07'] # 可用地图列表 + self.current_weather = 'clear' # 当前天气 + # 简化天气预设，使用肯定存在的天气类型 + self.weather_presets = { + 'clear': carla.WeatherParameters.ClearNoon, + 'rain': carla.WeatherParameters.HardRainNoon, + 'cloudy': carla.WeatherParameters.CloudyNoon, + 'wet': carla.WeatherParameters.WetNoon + } # 天气预设 + self.car_colors = [ + (255, 0, 0), # 红色 + (0, 0, 255), # 蓝色 + (0, 255, 0), # 绿色 + (255, 255, 0), # 黄色 + (255, 0, 255), # 品红色 + (0, 255, 255), # 青色 + (128, 0, 128), # 紫色 + (255, 165, 0), # 橙色 + (128, 128, 128), # 灰色 + (255, 255, 255) # 白色 + ] # 车辆颜色列表 + self.current_color_index = 0 # 当前颜色索引 + self.screenshot_dir = 'screenshots' # 截图保存目录 + 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 # 存储车辆出生点 + self.show_hud = False # HUD显示标志 + + def draw_hud(self, display_img, speed, throttle, steer, frame_count): + """绘制HUD仪表盘 + 在画面右上角绘制仪表盘，包含速度表、油门、方向盘等信息 + """ + try: + # 创建HUD背景（半透明黑色） + hud_height = 300 + hud_width = 280 + hud_bg = np.zeros((hud_height, hud_width, 4), dtype=np.uint8) + + # 绘制HUD背景 + cv2.rectangle(hud_bg, (0, 0), (hud_width, hud_height), (40, 40, 40, 200), -1) + cv2.rectangle(hud_bg, (0, 0), (hud_width, hud_height), (100, 100, 100), 2) + + # 绘制速度表（圆形仪表盘） + center_x, center_y = hud_width // 2, 100 + radius = 70 + + # 外圈 + cv2.circle(hud_bg, (center_x, center_y), radius, (80, 80, 80), 2) + + # 绘制速度刻度（0-200 km/h） + for i in range(0, 201, 20): + angle = np.radians(135 - (i / 200) * 270) # 从135度到405度（270度范围） + x1 = int(center_x + (radius - 10) * np.cos(angle)) + y1 = int(center_y - (radius - 10) * np.sin(angle)) + x2 = int(center_x + radius * np.cos(angle)) + y2 = int(center_y - radius * np.sin(angle)) + + # 重要刻度（0, 40, 80, 120, 160, 200） + if i % 40 == 0: + cv2.line(hud_bg, (x1, y1), (x2, y2), (255, 255, 255), 2) + # 添加数字 + text_x = int(center_x + (radius - 25) * np.cos(angle)) + text_y = int(center_y - (radius - 25) * np.sin(angle)) + cv2.putText(hud_bg, str(i), (text_x - 10, text_y + 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1) + else: + cv2.line(hud_bg, (x1, y1), (x2, y2), (80, 80, 80), 1) + + # 绘制速度指针 + speed_angle = np.radians(135 - (min(speed, 200) / 200) * 270) + needle_x = int(center_x + (radius - 20) * np.cos(speed_angle)) + needle_y = int(center_y - (radius - 20) * np.sin(speed_angle)) + cv2.line(hud_bg, (center_x, center_y), (needle_x, needle_y), (0, 255, 0), 3) + cv2.circle(hud_bg, (center_x, center_y), 8, (255, 255, 255), -1) + + # 显示速度数字（大字体） + cv2.putText(hud_bg, f"{int(speed)}", (center_x - 35, center_y + 55), + cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 2) + cv2.putText(hud_bg, "km/h", (center_x - 15, center_y + 75), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1) + + # 绘制油门进度条 + throttle_bar_width = 100 + throttle_bar_height = 15 + throttle_x = 20 + throttle_y = hud_height - 80 + + # 标签 + cv2.putText(hud_bg, "THROTTLE", (throttle_x, throttle_y - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1) + + # 背景条 + cv2.rectangle(hud_bg, (throttle_x, throttle_y), + (throttle_x + throttle_bar_width, throttle_y + throttle_bar_height), + (80, 80, 80), -1) + + # 油门条（绿色渐变表示力度） + throttle_width = int(throttle_bar_width * min(throttle, 1.0)) + if throttle_width > 0: + cv2.rectangle(hud_bg, (throttle_x, throttle_y), + (throttle_x + throttle_width, throttle_y + throttle_bar_height), + (0, 200, 0), -1) + + # 绘制刹车进度条 + brake_x = hud_width - throttle_bar_width - 20 + + # 标签 + cv2.putText(hud_bg, "BRAKE", (brake_x, throttle_y - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1) + + # 背景条 + cv2.rectangle(hud_bg, (brake_x, throttle_y), + (brake_x + throttle_bar_width, throttle_y + throttle_bar_height), + (80, 80, 80), -1) + + # 方向盘指示器 + steer_y = hud_height - 40 + cv2.putText(hud_bg, f"STEER: {steer:.2f}", (20, steer_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + # 左右转向指示 + if abs(steer) > 0.1: + direction = "LEFT" if steer < 0 else "RIGHT" + color = (0, 255, 255) if abs(steer) > 0.5 else (255, 255, 0) + cv2.putText(hud_bg, direction, (hud_width - 80, steer_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2) + + # 帧率信息 + cv2.putText(hud_bg, f"FPS: {frame_count % 60 + 1}", (20, steer_y + 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1) + + # 将HUD叠加到显示图像上（右上角） + # 获取图像尺寸 + img_height, img_width = display_img.shape[:2] + + # 创建ROI（右上角区域） + roi = display_img[0:hud_height, img_width - hud_width:img_width] + + # 混合HUD和原图 + alpha = 0.9 # HUD透明度 + cv2.addWeighted(hud_bg[:, :, :3], alpha, roi, 1 - alpha, 0, roi) + + return display_img + + except Exception as e: + print(f"绘制HUD时出错: {e}") + return display_img def connect(self): """连接到CARLA服务器""" @@ -143,20 +307,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() @@ -168,6 +337,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) @@ -184,6 +354,7 @@ def spawn_vehicle(self): if self.vehicle: print(f"车辆生成成功！ID: {self.vehicle.id}") + print(f"车辆型号: {vehicle_display_name}") print(f"位置: {spawn_point.location}") # 禁用自动驾驶 @@ -267,90 +438,418 @@ def update_camera_view(self): """更新相机视角""" print(f"已切换到{self.get_view_name()}视角") - def get_view_name(self): - """获取视角名称""" - view_names = { - 'first_person': 'First Person', - 'third_person': 'Third Person', - 'birdseye': 'Birds Eye' - } - return view_names.get(self.current_view, 'Unknown') + def switch_map(self): + """切换到下一个地图""" + try: + # 停止所有相机 + for view_mode, camera in self.cameras.items(): + if camera: + try: + camera.stop() + camera.destroy() + except: + pass + self.cameras.clear() + + # 销毁车辆 + if self.vehicle: + try: + self.vehicle.destroy() + except: + pass + self.vehicle = None + + # 等待清理完成 + time.sleep(1.0) + + # 切换到下一个地图 + current_index = self.available_maps.index(self.current_map) + next_index = (current_index + 1) % len(self.available_maps) + new_map = self.available_maps[next_index] + + print(f"正在加载地图: {new_map}...") + + # 完全重新连接CARLA客户端 + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) + + # 加载新地图 + self.world = self.client.load_world(new_map) + self.current_map = new_map + + # 等待地图完全加载 + time.sleep(3.0) + + # 重新生成车辆 + if not self.spawn_vehicle(): + raise Exception("车辆生成失败") + + # 重新设置相机 + if not self.setup_camera(): + raise Exception("相机设置失败") + + # 重新设置控制器 + self.setup_controller() + + # 重新生成NPC车辆 + self.spawn_npc_vehicles(2) + + # 应用当前天气 + self.set_weather(self.current_weather) + + print(f"地图切换成功: {self.current_map}") + + except Exception as e: + print(f"切换地图时出错: {e}") + # 尝试重新加载Town01作为备份 + try: + print("正在恢复到Town01...") + self.current_map = 'Town01' + time.sleep(1.0) + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) + self.world = self.client.load_world(self.current_map) + time.sleep(3.0) + self.spawn_vehicle() + self.setup_camera() + self.setup_controller() + self.set_weather(self.current_weather) + print("已恢复到Town01") + except Exception as e2: + print(f"恢复失败: {e2}") + + def set_weather(self, weather_type): + """设置天气""" + try: + if weather_type in self.weather_presets: + weather = self.weather_presets[weather_type] + self.world.set_weather(weather) + self.current_weather = weather_type + print(f"天气设置成功: {weather_type}") + return True + else: + print(f"无效的天气类型: {weather_type}") + return False + except Exception as e: + print(f"设置天气时出错: {e}") + return False - def setup_all_vehicles_cameras(self): - """为所有车辆设置相机系统""" - print("正在为所有车辆设置相机...") + def switch_weather(self): + """切换到下一个天气""" + try: + weather_types = list(self.weather_presets.keys()) + current_index = weather_types.index(self.current_weather) + next_index = (current_index + 1) % len(weather_types) + next_weather = weather_types[next_index] + self.set_weather(next_weather) + except Exception as e: + print(f"切换天气时出错: {e}") + def switch_color(self): + """切换车辆颜色""" try: - blueprint_library = self.world.get_blueprint_library() - camera_bp = blueprint_library.find('sensor.camera.rgb') + if self.vehicle: + # 获取当前车辆位置和方向 + transform = self.vehicle.get_transform() + + # 切换到下一个颜色 + self.current_color_index = (self.current_color_index + 1) % len(self.car_colors) + color = self.car_colors[self.current_color_index] + + # 获取颜色名称 + color_names = ['Red', 'Blue', 'Green', 'Yellow', 'Magenta', 'Cyan', 'Purple', 'Orange', 'Gray', 'White'] + color_name = color_names[self.current_color_index] + + # 停止相机 + for view_mode, camera in self.cameras.items(): + if camera: + try: + camera.stop() + camera.destroy() + except: + pass + self.cameras.clear() + + # 销毁当前车辆 + self.vehicle.destroy() + self.vehicle = None + + # 创建新车辆蓝图，使用当前选中的品牌 + blueprint_library = self.world.get_blueprint_library() + 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]}') + + # 首先尝试在相同位置生成新车辆 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, transform) + + # 如果失败，尝试使用出生点 + if not self.vehicle: + spawn_points = self.world.get_map().get_spawn_points() + for spawn_point in spawn_points[:5]: # 尝试前5个出生点 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if self.vehicle: + print("车辆已移动到新位置") + break + + if self.vehicle: + # 禁用自动驾驶 + self.vehicle.set_autopilot(False) + + # 重新设置相机 + self.setup_camera() + + # 重新设置控制器 + self.setup_controller() + + print(f"车辆颜色已切换: {color_name}") + else: + print("无法生成新车辆，颜色切换失败") + # 重置颜色索引 + self.current_color_index = (self.current_color_index - 1) % len(self.car_colors) + # 尝试恢复车辆 + self.spawn_vehicle() + else: + print("车辆不存在，无法切换颜色") + except Exception as e: + print(f"切换车辆颜色时出错: {e}") + # 重置颜色索引 + self.current_color_index = (self.current_color_index - 1) % len(self.car_colors) + # 尝试恢复车辆 + if not self.vehicle: + self.spawn_vehicle() - # 设置相机属性 - camera_bp.set_attribute('image_size_x', '640') - camera_bp.set_attribute('image_size_y', '480') - camera_bp.set_attribute('fov', '90') + 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: + camera.stop() + camera.destroy() + except: + pass + self.cameras.clear() + + # 销毁当前车辆 + self.vehicle.destroy() + self.vehicle = None + + # 创建新车辆蓝图 + blueprint_library = self.world.get_blueprint_library() + vehicle_bp = blueprint_library.find(self.vehicle_models[self.current_vehicle_index][0]) + if not vehicle_bp: + 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 self.vehicle: + # 禁用自动驾驶 + self.vehicle.set_autopilot(False) + + # 重新设置相机 + self.setup_camera() + + # 重新设置控制器 + self.setup_controller() + + print(f"\n车辆品牌已切换: {new_vehicle_name}") + else: + print("无法生成新车辆，品牌切换失败") + # 尝试恢复车辆 + self.spawn_vehicle() + else: + print("车辆不存在，无法切换品牌") + + except Exception as e: + print(f"切换车辆品牌时出错: {e}") + # 尝试恢复车辆 + if not self.vehicle: + self.spawn_vehicle() - # 为每辆车创建相机 - for i, vehicle in enumerate(self.vehicles): - if vehicle is None: - continue + 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}") - print(f"为车辆 {i} 设置相机...") + 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) - # 第三人称相机 - 用于跟随视角 - 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 + def toggle_night_mode(self): + """切换夜晚模式并自动打开/关闭近光灯""" + try: + # 检查是否已有夜晚模式标志 + if not hasattr(self, 'is_night_mode'): + self.is_night_mode = False + + 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 ) - 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 - + 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}") - return False + print(f"切换夜晚模式时出错: {e}") - def camera_callback(self, image, vehicle_index, view_mode=None): - """相机数据回调""" + def take_screenshot(self, image): + """保存当前画面截图""" 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 + import os + import time + + # 创建截图目录 + os.makedirs(self.screenshot_dir, exist_ok=True) + + # 获取当前时间戳 + timestamp = time.strftime("%Y%m%d_%H%M%S") + + # 获取当前地图名称 + map_name = self.current_map + + # 获取当前天气 + weather_name = self.current_weather + + # 获取当前颜色名称 + color_names = ['Red', 'Blue', 'Green', 'Yellow', 'Magenta', 'Cyan', 'Purple', 'Orange', 'Gray', 'White'] + color_name = color_names[self.current_color_index] + + # 生成文件名 + filename = f"screenshot_{timestamp}_{map_name}_{weather_name}_{color_name}.png" + filepath = os.path.join(self.screenshot_dir, filename) + + # 保存截图 + cv2.imwrite(filepath, image) + + print(f"截图已保存: {filepath}") + + except Exception as e: + print(f"保存截图时出错: {e}") - 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 get_view_name(self): + """获取视角名称""" + view_names = { + 'first_person': 'First Person', + 'third_person': 'Third Person', + 'birdseye': 'Birds Eye' + } + return view_names.get(self.current_view, 'Unknown') 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) @@ -365,6 +864,11 @@ def run(self): if not self.spawn_vehicle(): return + # 设置相机 + if not self.setup_camera(): + # 即使相机失败也继续运行 + print("警告：相机设置失败，继续运行...") + # 设置控制器 self.setup_controller() @@ -380,104 +884,131 @@ def run(self): ) self.world.set_weather(weather) - # 生成2辆NPC车辆（加上主车辆共3辆特斯拉） - npc_vehicles = self.spawn_npc_vehicles(2) - - # 将所有车辆添加到列表中（主车辆在前，NPC在后） - self.vehicles = [self.vehicle] + npc_vehicles - - # 为所有车辆设置控制器 - self.setup_all_controllers() + # 生成一些NPC车辆 + self.spawn_npc_vehicles(2) print("\n系统准备就绪！") - print(f"共 {len(self.vehicles)} 辆车可用") print("控制指令:") print(" q - 退出程序") - print(" r - 重置当前车辆") + print(" r - 重置车辆") print(" s - 紧急停止") print(" x - 切换倒车/前进模式（速度为0时生效）") - for i in range(len(self.vehicles)): - if i == 0: - print(f" 1 - 切换到主车辆视角（红色特斯拉）") - else: - print(f" {i+1} - 切换到NPC车辆{i}视角") + print(" v - 切换视角（第一人称/第三人称/鸟瞰图）") + print(" m - 切换地图（Town01/Town02/Town03等）") + print(" w - 切换天气（晴天/雨天/多云/湿滑）") + print(" c - 切换车辆颜色") + print(" u - 切换车辆款式（Tesla/Chevrolet/Ford等）") + print(" l - 切换夜晚模式（自动打开/关闭近光灯）") + print(" d - 切换导航轨迹显示") + print(" y - 切换HUD仪表盘显示") + print(" p - 保存当前画面截图") 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: - # 获取当前关注车辆的状态 - current_vehicle = self.vehicles[self.current_vehicle_index] - velocity = current_vehicle.get_velocity() + # 获取车辆状态 + velocity = self.vehicle.get_velocity() speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 - # 检测主车辆是否卡住 - 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 + # 获取控制指令（现在返回4个值，原代码返回3个值） + # throttle, brake, steer = self.controller.get_control() # 原代码 + throttle, brake, steer, reverse = self.controller.get_control() # 新代码 - # 对所有车辆应用控制器 - # 主车辆（索引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=reverse + # reverse=False # 原代码 + 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, 60), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (255, 255, 255), 2) + (20, 40), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) cv2.putText(display_img, f"Throttle: {throttle:.2f}", - (20, 90), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (255, 255, 255), 2) + (20, 80), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) cv2.putText(display_img, f"Steer: {steer:.2f}", (20, 120), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (255, 255, 255), 2) + 0.8, (255, 255, 255), 2) cv2.putText(display_img, f"Frame: {frame_count}", - (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) + (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.putText(display_img, f"Map: {self.current_map}", + (20, 280), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 0), 2) # 黄色显示 + + # 显示当前天气 + cv2.putText(display_img, f"Weather: {self.current_weather}", + (20, 320), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 0, 255), 2) # 品红色显示 + + # 显示当前车辆颜色 + color_names = ['Red', 'Blue', 'Green', 'Yellow', 'Magenta', 'Cyan', 'Purple', 'Orange', 'Gray', 'White'] + current_color_name = color_names[self.current_color_index] + cv2.putText(display_img, f"Color: {current_color_name}", + (20, 360), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (0, 128, 255), 2) # 橙色显示 + + # 显示轨迹导航（新功能） + if self.show_trajectory: + # 更新轨迹点 + location = self.vehicle.get_location() + self.trajectory_points.append((location.x, location.y)) + + # 如果地图图像不存在，生成它 + if self.map_img is None: + self.generate_topology_map() + + # 绘制轨迹地图 + if 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)): + prev_point = self.world_to_map(carla.Location(x=self.trajectory_points[i-1][0], y=self.trajectory_points[i-1][1])) + curr_point = self.world_to_map(carla.Location(x=self.trajectory_points[i][0], y=self.trajectory_points[i][1])) + cv2.line(map_display, prev_point, curr_point, (0, 255, 0), 2) + + # 绘制当前车辆位置（红色圆点） + vehicle_pos = self.world_to_map(location) + cv2.circle(map_display, vehicle_pos, 8, (0, 0, 255), -1) + cv2.circle(map_display, vehicle_pos, 12, (0, 0, 255), 2) + + # 调整地图大小并显示在角落 + map_resized = cv2.resize(map_display, (200, 200)) + display_img[-220:-20, -220:-20] = map_resized + + # 显示HUD仪表盘 + if self.show_hud: + display_img = self.draw_hud(display_img, speed, throttle, steer, frame_count) + + cv2.imshow('Autonomous Driving - Simple Version', display_img) # 处理按键 key = cv2.waitKey(1) & 0xFF @@ -485,30 +1016,64 @@ def run(self): print("正在退出...") running = False elif key == ord('r'): - self.reset_current_vehicle() + self.reset_vehicle() elif key == ord('s'): - # 紧急停止（只对主车辆生效） - if self.current_vehicle_index == 0: - self.vehicle.apply_control(carla.VehicleControl( - throttle=0.0, brake=1.0, hand_brake=True - )) + # 紧急停止 + self.vehicle.apply_control(carla.VehicleControl( + throttle=0.0, brake=1.0, hand_brake=True + )) print("紧急停止") elif key == ord('x'): # 切换倒车模式（只在速度接近0时允许切换） - if self.current_vehicle_index == 0 and speed < 1.0: + if speed < 1.0: # 速度小于1km/h时允许切换 self.controller.toggle_reverse() - elif self.current_vehicle_index != 0: - print("只有主车辆可以切换倒车模式") else: print("请先减速到接近停止（速度<1km/h）再切换倒车模式") - 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() + 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 key == ord('m'): + # 切换地图 + self.switch_map() + elif key == ord('w'): + # 切换天气 + self.switch_weather() + 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() + elif key == ord('d') or key == ord('D'): + # 切换轨迹显示（支持大小写） + self.show_trajectory = not self.show_trajectory + if self.show_trajectory: + print("轨迹显示已开启") else: - print(f"车辆 {vehicle_index + 1} 不存在") + print("轨迹显示已关闭") + elif key == ord('y') or key == ord('Y'): + # 切换HUD仪表盘显示（支持大小写） + self.show_hud = not self.show_hud + if self.show_hud: + print("HUD仪表盘已开启") + else: + print("HUD仪表盘已关闭") frame_count += 1 @@ -526,65 +1091,40 @@ 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(count): - # 使用不同的出生点 - spawn_index = i % len(far_spawn_points) + for i in range(min(count, len(spawn_points))): + # 跳过主车辆的出生点 + if i == 0: + continue try: - # 使用特斯拉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}") + # 随机选择车辆类型 + 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: pass print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") - return npc_vehicles except Exception as e: print(f"生成NPC车辆时出错: {e}") - return [] def reset_vehicle(self): """重置车辆位置""" @@ -592,38 +1132,10 @@ def reset_vehicle(self): spawn_points = self.world.get_map().get_spawn_points() if 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) - + 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)