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() diff --git a/src/Unmanned_car_security/obstacle_detector.py b/src/Unmanned_car_security/obstacle_detector.py new file mode 100644 index 0000000000..fad5a2626b --- /dev/null +++ b/src/Unmanned_car_security/obstacle_detector.py @@ -0,0 +1,76 @@ +# obstacle_detector.py +import numpy as np + + +class ObstacleDetector: + """简洁版障碍物检测器""" + + def __init__(self): + self.obstacles = [] + self.warning_level = 0 # 0=无, 1=注意, 2=警告, 3=危险 + self.warning_message = "" # 初始化时设置空字符串 + + def detect(self, point_cloud): + """检测障碍物 - 简化版本""" + self.obstacles = [] + + if point_cloud is None or len(point_cloud) < 10: + self.warning_level = 0 + self.warning_message = "前方区域清晰" + return self.obstacles + + try: + # 1. 简化的检测逻辑:统计前方10米内的点 + forward_points = point_cloud[ + (point_cloud[:, 0] > 0) & # 前方 + (point_cloud[:, 0] < 10) & # 10米内 + (np.abs(point_cloud[:, 1]) < 3) # 左右3米内 + ] + + if len(forward_points) > 50: # 足够多的点才认为是障碍物 + # 计算平均距离 + avg_distance = np.mean(forward_points[:, 0]) + + # 找到最近的障碍物 + min_distance = np.min(forward_points[:, 0]) + + self.obstacles.append({ + 'distance': avg_distance, + 'min_distance': min_distance, + 'point_count': len(forward_points) + }) + + # 设置警告级别和消息 + if min_distance < 2.0: + self.warning_level = 3 + self.warning_message = f"危险!前方{min_distance:.1f}米处有障碍物" + elif min_distance < 4.0: + self.warning_level = 2 + self.warning_message = f"警告:前方{min_distance:.1f}米处有障碍物" + elif min_distance < 7.0: + self.warning_level = 1 + self.warning_message = f"注意:前方{min_distance:.1f}米处有障碍物" + else: + self.warning_level = 0 + self.warning_message = "前方区域清晰" + else: + self.warning_level = 0 + self.warning_message = "前方区域清晰" + + except Exception as e: + print(f"障碍物检测出错: {e}") + self.warning_level = 0 + self.warning_message = "检测系统异常" + + return self.obstacles + + def get_warning_color(self): + """获取警告颜色""" + if self.warning_level == 0: + return (0, 255, 0) # 绿色 + elif self.warning_level == 1: + return (255, 255, 0) # 黄色 + elif self.warning_level == 2: + return (255, 165, 0) # 橙色 + else: # level 3 + return (255, 0, 0) # 红色 \ No newline at end of file