diff --git a/src/Unmanned_car_security/drawer.py b/src/Unmanned_car_security/drawer.py index 4ab288d712..657afdfe40 100644 --- a/src/Unmanned_car_security/drawer.py +++ b/src/Unmanned_car_security/drawer.py @@ -19,11 +19,41 @@ def display_location(self, location): """显示位置信息""" if location: location_text = self.small_font.render( - f'Location: ({location.x:.1f}, {location.y:.1f}, {location.z:.1f})', + f'Location: ({location.x:.1f}, {location.y:.1f})', True, (255, 255, 255) ) self.screen.blit(location_text, (20, 70)) + def display_warning(self, warning_message, color, warning_level): + """显示障碍物警告信息""" + # 显示警告信息在速度下方 + if warning_message: + warning_text = self.small_font.render(warning_message, True, color) + self.screen.blit(warning_text, (20, 110)) + + # 显示警告级别 + level_texts = ["安全", "注意", "警告", "危险"] + level_text = self.small_font.render(f"警告级别: {level_texts[warning_level]}", + True, color) + self.screen.blit(level_text, (20, 140)) + + # 绘制一个简单的状态指示器 + indicator_width = 300 + indicator_height = 20 + indicator_x = 20 + indicator_y = 170 + + # 绘制背景条 + pygame.draw.rect(self.screen, (60, 60, 60), + (indicator_x, indicator_y, indicator_width, indicator_height), + border_radius=5) + + # 根据警告级别绘制不同长度的彩色条 + if warning_level > 0: + fill_width = int(indicator_width * (warning_level / 3)) + fill_rect = pygame.Rect(indicator_x, indicator_y, fill_width, indicator_height) + pygame.draw.rect(self.screen, color, fill_rect, border_radius=5) + def draw_camera(self, image_array): """绘制摄像头图像(如果需要)""" # 这里可以添加摄像头图像的绘制逻辑 diff --git a/src/Unmanned_car_security/main.py b/src/Unmanned_car_security/main.py index 5c66587f5b..2c8c42dc7e 100644 --- a/src/Unmanned_car_security/main.py +++ b/src/Unmanned_car_security/main.py @@ -34,6 +34,8 @@ from drawer import PyGameDrawer from sync_pygame import SyncPyGame +# 导入障碍物检测器 +from obstacle_detector import ObstacleDetector class Main(): @@ -65,6 +67,12 @@ def __init__(self): print("🚘 生成自动驾驶车辆...") self.spawn_vehicle() + # 添加障碍物检测器 + self.obstacle_detector = ObstacleDetector() + + # 用于存储最新的激光雷达数据 + self.latest_lidar_data = None + # 安装传感器 self.setup_lidar() self.setup_camera() @@ -75,6 +83,7 @@ def __init__(self): # 开始游戏循环 print("▶️ 启动自动驾驶...") print("📊 车辆速度和位置将显示在屏幕上") + print("🚧 障碍物检测系统已启用") print("ℹ️ 按ESC键退出程序") print("=" * 50) @@ -143,6 +152,7 @@ def setup_lidar(self): 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: @@ -164,11 +174,14 @@ def setup_camera(self): 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)) + # 存储最新的激光雷达数据用于障碍物检测 + self.latest_lidar_data = point_cloud + # 减少控制台输出频率,避免过于频繁 if random.random() < 0.01: # 1%的概率输出 print(f"📡 激光雷达点云: {len(point_cloud)} 个点") @@ -199,9 +212,24 @@ def on_tick(self): speed_m_s = np.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) speed_kmh = speed_m_s * 3.6 + # 障碍物检测 + if self.latest_lidar_data is not None: + obstacles = self.obstacle_detector.detect(self.latest_lidar_data) + + # 定期输出检测结果(避免控制台太拥挤) + if random.random() < 0.05: # 5%概率输出 + if self.obstacle_detector.warning_level > 0: + print(f"🚧 {self.obstacle_detector.warning_message}") + # 更新绘制器显示 self.drawer.display_speed(speed_kmh) self.drawer.display_location(location) + # 显示障碍物警告信息 + self.drawer.display_warning( + self.obstacle_detector.warning_message, + self.obstacle_detector.get_warning_color(), + self.obstacle_detector.warning_level + ) # 更新观察者视角跟随车辆 self.update_spectator()