diff --git a/src/Unmanned_car_security/main.py b/src/Unmanned_car_security/main.py index 6504182890..5c66587f5b 100644 --- a/src/Unmanned_car_security/main.py +++ b/src/Unmanned_car_security/main.py @@ -1,286 +1,273 @@ -import sys -import os +#!/usr/bin/env python3 + import carla -import time import numpy as np +import time import random +import sys +import os - -# -------------------------- 1. 智能查找 Carla 路径 --------------------------- -def find_carla_path(): - """智能查找 CARLA 安装路径""" - - # 先检查环境变量 - if 'CARLA_ROOT' in os.environ: - base_path = os.environ['CARLA_ROOT'] - print(f"📁 使用环境变量 CARLA_ROOT: {base_path}") +# 添加CARLA PythonAPI路径 +try: + # 尝试自动查找CARLA路径 + possible_paths = [ + "D:/CARLA_0.9.10/WindowsNoEditor/PythonAPI/carla", + "D:/CARLA_0.9.11/WindowsNoEditor/PythonAPI/carla", + "D:/CARLA_0.9.12/WindowsNoEditor/PythonAPI/carla", + "D:/CARLA_0.9.13/WindowsNoEditor/PythonAPI/carla", + "D:/CARLA_0.9.14/WindowsNoEditor/PythonAPI/carla", + "C:/CARLA_0.9.10/WindowsNoEditor/PythonAPI/carla", + ] + + for path in possible_paths: + if os.path.exists(path): + if path not in sys.path: + sys.path.append(path) + print(f"✅ 添加CARLA路径: {path}") + break else: - # 常见的安装路径 - possible_paths = [ - "D:/CARLA_0.9.10", - "D:/CARLA_0.9.11", - "D:/CARLA_0.9.12", - "D:/CARLA_0.9.13", - "D:/CARLA_0.9.14", - "C:/CARLA_0.9.10", - "C:/CARLA_0.9.11", - "C:/CARLA_0.9.12", - "C:/CARLA_0.9.13", - "C:/CARLA_0.9.14", - # 默认路径 - "D:/CARLA_0.9.10" - ] - - base_path = None - for path in possible_paths: - if os.path.exists(path): - base_path = path - print(f"📁 自动检测到 CARLA 路径: {path}") - break - - if not base_path: - print("❌ 未找到 CARLA 安装路径") - print(" 请将 CARLA 安装在以下位置之一:") - for path in possible_paths[:5]: - print(f" - {path}") - return None - - # 构建 PythonAPI 完整路径 - carla_api_path = os.path.join(base_path, "WindowsNoEditor", "PythonAPI") - - if not os.path.exists(carla_api_path): - print(f"❌ PythonAPI 路径不存在: {carla_api_path}") - return None - - return carla_api_path - - -# 查找并设置 CARLA 路径 -carla_api_path = find_carla_path() -if not carla_api_path: - sys.exit(1) - -if carla_api_path not in sys.path: - sys.path.append(carla_api_path) - print(f"✅ CARLA 路径设置成功: {carla_api_path}") - -# -------------------------- 2. 全局变量 -------------------------- -HOST = "localhost" # Carla 服务器 IP(本地默认 localhost) -PORT = 2000 # Carla 服务器端口(默认 2000) -VEHICLE_MODEL = "model3" # 车辆模型(可改为 "cybertruck"、"mustang" 等) -LIDAR_RANGE = 50 # 激光雷达探测范围(米) - -# 全局对象(后续会初始化) -client = None -world = None -vehicle = None -lidar_sensor = None -camera_sensor = None - - -# -------------------------- 3. 核心功能函数 -------------------------- -def connect_to_carla(): - """连接到 Carla 服务器""" - global client, world - try: - # 创建客户端并连接 - client = carla.Client(HOST, PORT) - client.set_timeout(10.0) # 超时时间(10 秒) - world = client.get_world() # 获取 Carla 世界对象 - - print(f"✅ 成功连接到 Carla!当前地图:{world.get_map().name}") - except Exception as e: - print(f"❌ 连接 Carla 失败:{e}") + print("❌ 未找到CARLA路径,请手动设置") sys.exit(1) +except Exception as e: + print(f"⚠️ 路径设置警告: {e}") + +from drawer import PyGameDrawer +from sync_pygame import SyncPyGame + + +class Main(): + def __init__(self): + # 配置参数 + self.CARLA_SERVER = "localhost" + self.PORT = 2000 + self.VEHICLE_MODEL = "model3" + self.LIDAR_RANGE = 50 + + print("=" * 50) + print("🚗 CARLA 自动驾驶模拟器") + print("=" * 50) + + try: + # 连接Carla服务器 + print("🔄 连接到Carla服务器...") + self.client = carla.Client(self.CARLA_SERVER, self.PORT) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + self.map = self.world.get_map() + print(f"✅ 已连接,当前地图: {self.map.name}") + + # 初始化Pygame + print("🎮 初始化Pygame界面...") + self.game = SyncPyGame(self) + + # 生成主车辆 + print("🚘 生成自动驾驶车辆...") + self.spawn_vehicle() + + # 安装传感器 + self.setup_lidar() + self.setup_camera() + + # 初始化绘制器 + self.drawer = PyGameDrawer(self) + + # 开始游戏循环 + print("▶️ 启动自动驾驶...") + print("📊 车辆速度和位置将显示在屏幕上") + print("ℹ️ 按ESC键退出程序") + print("=" * 50) + + self.game.game_loop(self.world, self.on_tick) + + except Exception as e: + print(f"❌ 初始化失败: {e}") + import traceback + traceback.print_exc() + self.cleanup() + + def spawn_vehicle(self): + """生成车辆""" + try: + # 获取所有生成点 + spawn_points = self.map.get_spawn_points() + if not spawn_points: + raise Exception("地图中没有可用的生成点") + + # 选择一个生成点 + spawn_point = random.choice(spawn_points) + + # 获取车辆蓝图 + blueprint_lib = self.world.get_blueprint_library() + vehicle_bp = blueprint_lib.filter(self.VEHICLE_MODEL) + + if not vehicle_bp: + # 如果指定车型不存在,使用第一个可用的 + vehicle_bp = blueprint_lib.filter("vehicle.*")[0] + print(f"⚠️ 车辆 '{self.VEHICLE_MODEL}' 不存在,使用默认车辆") + else: + vehicle_bp = vehicle_bp[0] + + # 生成车辆 + self.ego = self.world.try_spawn_actor(vehicle_bp, spawn_point) + + if not self.ego: + # 如果生成失败,尝试其他位置 + for point in spawn_points: + self.ego = self.world.try_spawn_actor(vehicle_bp, point) + if self.ego: + spawn_point = point + break + + if not self.ego: + raise Exception("无法生成车辆,请检查地图和生成点") + + print(f"✅ 车辆已生成在位置: ({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f})") + + # 启用自动驾驶 + self.ego.set_autopilot(True) + print("🚦 自动驾驶已启用") + + except Exception as e: + print(f"❌ 生成车辆失败: {e}") + raise + + def setup_lidar(self): + """安装激光雷达传感器""" + try: + lidar_bp = self.world.get_blueprint_library().find("sensor.lidar.ray_cast") + lidar_bp.set_attribute("range", str(self.LIDAR_RANGE)) + lidar_bp.set_attribute("points_per_second", "50000") + lidar_bp.set_attribute("rotation_frequency", "10") + lidar_bp.set_attribute("channels", "32") + + lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.4)) + self.lidar = self.world.spawn_actor(lidar_bp, lidar_transform, attach_to=self.ego) + self.lidar.listen(lambda data: self.process_lidar(data)) + print("✅ 激光雷达已安装") + except Exception as e: + print(f"⚠️ 安装激光雷达失败: {e}") + + def setup_camera(self): + """安装摄像头传感器""" + try: + camera_bp = self.world.get_blueprint_library().find("sensor.camera.rgb") + camera_bp.set_attribute("image_size_x", "800") + camera_bp.set_attribute("image_size_y", "600") + camera_bp.set_attribute("fov", "110") + + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.ego) + self.camera.listen(lambda image: self.process_camera(image)) + print("✅ 摄像头已安装") + except Exception as e: + print(f"⚠️ 安装摄像头失败: {e}") + + def process_lidar(self, data): + """处理激光雷达数据""" + try: + point_cloud = np.frombuffer(data.raw_data, dtype=np.dtype('f4')) + point_cloud = np.reshape(point_cloud, (int(point_cloud.shape[0] / 4), 4)) + + # 减少控制台输出频率,避免过于频繁 + if random.random() < 0.01: # 1%的概率输出 + print(f"📡 激光雷达点云: {len(point_cloud)} 个点") + + except Exception as e: + pass + + def process_camera(self, image): + """处理摄像头数据""" + try: + # 将CARLA图像转换为numpy数组 + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + + except Exception as e: + pass + + def on_tick(self): + """每一帧调用的主函数""" + try: + # 获取车辆状态 + if hasattr(self, 'ego') and self.ego: + location = self.ego.get_location() + velocity = self.ego.get_velocity() + + # 计算速度 (m/s 转换为 km/h) + speed_m_s = np.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + speed_kmh = speed_m_s * 3.6 + + # 更新绘制器显示 + self.drawer.display_speed(speed_kmh) + self.drawer.display_location(location) + + # 更新观察者视角跟随车辆 + self.update_spectator() + + except Exception as e: + print(f"⚠️ 更新车辆状态失败: {e}") + + def update_spectator(self): + """更新观察者视角""" + try: + spectator = self.world.get_spectator() + transform = self.ego.get_transform() + + # 计算观察者位置(车辆后方10米,上方5米) + location = transform.location + rotation = transform.rotation + + x = location.x - 10 * np.cos(np.radians(rotation.yaw)) + y = location.y - 10 * np.sin(np.radians(rotation.yaw)) + z = location.z + 5 + + spectator.set_transform(carla.Transform( + carla.Location(x=x, y=y, z=z), + carla.Rotation(pitch=-20, yaw=rotation.yaw) + )) + except Exception as e: + pass + + def cleanup(self): + """清理资源""" + print("\n🧹 开始清理资源...") -def spawn_vehicle(): - """在 Carla 中生成车辆""" - global vehicle - try: - # 获取车辆蓝图库 - blueprint_library = world.get_blueprint_library() - vehicle_bp = blueprint_library.filter(VEHICLE_MODEL)[0] # 选择车辆模型 - - # 获取地图中的生成点(选择第一个可用生成点) - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: - raise Exception("❌ 地图中没有可用的车辆生成点") - - # 随机选择一个生成点 - spawn_point = random.choice(spawn_points) - - # 生成车辆 - vehicle = world.spawn_actor(vehicle_bp, spawn_point) - vehicle.set_autopilot(True) # 开启自动驾驶,让车辆自动行驶 - - print(f"✅ 成功生成车辆:{VEHICLE_MODEL}(位置:{spawn_point.location})") - - # 等待一下,确保车辆完全生成 - time.sleep(1) - - except Exception as e: - print(f"❌ 生成车辆失败:{e}") - sys.exit(1) - - -def setup_lidar(): - """为车辆安装激光雷达传感器""" - global lidar_sensor - try: - # 获取激光雷达蓝图 - blueprint_library = world.get_blueprint_library() - lidar_bp = blueprint_library.find("sensor.lidar.ray_cast") - - # 配置激光雷达参数 - lidar_bp.set_attribute("range", str(LIDAR_RANGE)) # 探测范围 - lidar_bp.set_attribute("points_per_second", "50000") # 每秒点数 - lidar_bp.set_attribute("rotation_frequency", "10") # 旋转频率(Hz) - lidar_bp.set_attribute("channels", "32") # 通道数 - - # 激光雷达安装位置(车辆顶部,x 向前,z 向上) - lidar_transform = carla.Transform(carla.Location(x=0.0, z=2.4)) - - # 生成激光雷达并挂载到车辆上 - lidar_sensor = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) - - # 注册激光雷达数据回调函数(每帧数据都会触发) - lidar_sensor.listen(lambda data: lidar_callback(data)) - print(f"✅ 激光雷达已安装:探测范围 {LIDAR_RANGE} 米,回调函数已注册") - except Exception as e: - print(f"❌ 安装激光雷达失败:{e}") - sys.exit(1) - - -def setup_camera(): - """为车辆安装摄像头传感器(用于观察)""" - global camera_sensor - try: - # 获取摄像头蓝图 - blueprint_library = world.get_blueprint_library() - camera_bp = blueprint_library.find("sensor.camera.rgb") - - # 配置摄像头参数 - camera_bp.set_attribute("image_size_x", "800") - camera_bp.set_attribute("image_size_y", "600") - camera_bp.set_attribute("fov", "110") - - # 摄像头安装位置(车辆前方) - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) - - # 生成摄像头并挂载到车辆上 - camera_sensor = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) - - # 注册摄像头数据回调函数 - camera_sensor.listen(lambda image: camera_callback(image)) - print("✅ 摄像头已安装") - except Exception as e: - print(f"❌ 安装摄像头失败:{e}") - - -def camera_callback(image): - """摄像头数据回调函数""" - # 这里可以处理图像数据,但为了性能我们只是简单确认摄像头在工作 - pass - - -def set_spectator_follow_vehicle(): - """设置观察者视角跟随车辆""" - try: - # 获取观察者对象 - spectator = world.get_spectator() - - # 设置观察者位置在车辆后方上方 - def update_spectator(): - if vehicle: - transform = vehicle.get_transform() - # 计算观察者位置(车辆后方10米,上方5米) - location = transform.location - rotation = transform.rotation - - # 计算后方位置 - x = location.x - 10 * np.cos(np.radians(rotation.yaw)) - y = location.y - 10 * np.sin(np.radians(rotation.yaw)) - z = location.z + 5 - - spectator.set_transform(carla.Transform( - carla.Location(x=x, y=y, z=z), - carla.Rotation(pitch=-20, yaw=rotation.yaw) - )) - - return update_spectator - except Exception as e: - print(f"❌ 设置观察者视角失败:{e}") - return None - - -def lidar_callback(data): - """激光雷达数据回调函数(处理每帧点云数据)""" - try: - # 使用 raw_data 属性并将其转换为点云 - point_cloud = np.frombuffer(data.raw_data, dtype=np.dtype('f4')) - point_cloud = np.reshape(point_cloud, (int(point_cloud.shape[0] / 4), 4)) - - # 打印基本信息(减少输出频率,避免控制台过于拥挤) - if random.random() < 0.1: # 只有10%的概率输出,减少控制台输出 - print(f"📡 激光雷达帧数据:共 {len(point_cloud)} 个点") + # 销毁传感器 + if hasattr(self, 'camera') and self.camera: + try: + self.camera.destroy() + print("✅ 摄像头已销毁") + except: + pass + + if hasattr(self, 'lidar') and self.lidar: + try: + self.lidar.stop() + self.lidar.destroy() + print("✅ 激光雷达已销毁") + except: + pass + + # 销毁车辆 + if hasattr(self, 'ego') and self.ego: + try: + self.ego.destroy() + print("✅ 车辆已销毁") + except: + pass - except Exception as e: - print(f"❌ 处理激光雷达数据时出错:{e}") + print("🧹 资源清理完成!") -def main(): +if __name__ == '__main__': try: - # 1. 连接 Carla 服务器 - connect_to_carla() - # 2. 生成车辆 - spawn_vehicle() - # 3. 安装激光雷达 - setup_lidar() - # 4. 安装摄像头(可选) - setup_camera() - # 5. 设置观察者视角跟随车辆 - update_spectator = set_spectator_follow_vehicle() - - # 6. 保持程序运行(持续接收激光雷达数据) - print("\n⏳ 程序运行中,车辆将自动行驶,按 Ctrl+C 停止...") - - frame_count = 0 - while True: - # 每帧更新观察者视角 - if update_spectator: - update_spectator() - - frame_count += 1 - if frame_count % 100 == 0: # 每100帧输出一次状态 - if vehicle: - location = vehicle.get_location() - velocity = vehicle.get_velocity() - speed = 3.6 * (velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) ** 0.5 # 转换为km/h - print( - f"📍 车辆位置: x={location.x:.1f}, y={location.y:.1f}, z={location.z:.1f}, 速度: {speed:.1f} km/h") - - time.sleep(0.05) # 控制循环频率 - + Main() except KeyboardInterrupt: - print("\n\n🛑 程序被用户停止") - finally: - # 7. 清理资源(销毁车辆和传感器,避免 Carla 服务器残留) - print("\n🧹 开始清理资源...") - - if camera_sensor: - camera_sensor.destroy() - print("✅ 摄像头已销毁") - if lidar_sensor: - lidar_sensor.stop() - lidar_sensor.destroy() - print("✅ 激光雷达已销毁") - if vehicle: - vehicle.destroy() - print("✅ 车辆已销毁") - print("🧹 资源清理完成!") - + print("\n🛑 程序被用户停止") + except Exception as e: + print(f"\n❌ 程序运行出错: {e}") + import traceback -# -------------------------- 4. 运行主函数 -------------------------- -if __name__ == "__main__": - main() \ No newline at end of file + traceback.print_exc() \ No newline at end of file