diff --git a/src/Unmanned_car_perceive/drawer.py b/src/Unmanned_car_perceive/drawer.py index b3a49f56f2..fad5a2626b 100644 --- a/src/Unmanned_car_perceive/drawer.py +++ b/src/Unmanned_car_perceive/drawer.py @@ -1,115 +1,76 @@ -import pygame +# obstacle_detector.py import numpy as np -class PyGameDrawer: - def __init__(self, main): - self.main = main - self.screen = main.game.screen - self.font = pygame.font.SysFont('Arial', 32) - self.small_font = pygame.font.SysFont('Arial', 20) - self.warning_font = pygame.font.SysFont('Arial', 24, bold=True) - - # 摄像头图像存储 - self.camera_image = None - - def display_speed(self, speed): - """显示速度在屏幕左上角""" - # 创建速度文本 - speed_text = self.font.render(f'Speed: {speed:.1f} km/h', True, (255, 255, 255)) - self.screen.blit(speed_text, (20, 20)) - - def display_location(self, location): - """显示位置信息""" - if location: - location_text = self.small_font.render( - 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 display_camera(self): - """显示摄像头图像 - 新增功能""" - if self.camera_image is not None: - try: - # 摄像头图像显示在右上角 - cam_x = self.screen.get_width() - 420 # 右边距20，宽度400 - cam_y = 20 # 上边距 - - # 确保图像是有效的numpy数组 - if self.camera_image.shape[0] > 0 and self.camera_image.shape[1] > 0: - # 将numpy数组转换为Pygame表面 - # 注意：numpy数组是(height, width, 3)格式，需要转置为(width, height, 3) - image_surface = pygame.surfarray.make_surface(self.camera_image.swapaxes(0, 1)) - - # 调整图像大小以适应显示区域 - target_width = 400 - target_height = 300 - image_surface = pygame.transform.scale(image_surface, (target_width, target_height)) - - # 绘制图像 - self.screen.blit(image_surface, (cam_x, cam_y)) - - # 绘制边框和标题 - pygame.draw.rect(self.screen, (100, 100, 100), - (cam_x - 2, cam_y - 2, target_width + 4, target_height + 4), - 2, border_radius=5) - - # 添加摄像头标签 - camera_label = self.small_font.render("摄像头视图", True, (255, 255, 255)) - self.screen.blit(camera_label, (cam_x + 150, cam_y + target_height + 10)) - - except Exception as e: - # 如果绘制失败，静默处理（避免影响主程序） - # 可以在调试时取消注释下面的打印 - # print(f"绘制摄像头图像失败: {e}") - pass - - # 🆕 新增：帧率显示功能 - def display_fps(self, fps): - """在屏幕右上角显示实时帧率""" - fps_text = self.small_font.render(f'FPS: {fps:.1f}', True, (200, 200, 255)) - # 显示在右上角，摄像头图像的右侧 - fps_x = self.screen.get_width() - 120 # 右边距20，宽度100 - fps_y = 20 - self.screen.blit(fps_text, (fps_x, fps_y)) - - def draw_camera(self, image_array): - """绘制摄像头图像（兼容旧代码）""" - # 将图像存储起来供display_camera使用 - self.camera_image = image_array - - def draw_lidar(self, point_cloud): - """绘制激光雷达点云（如果需要）""" - # 这里可以添加点云的绘制逻辑 - pass \ No newline at end of file +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 diff --git a/src/Unmanned_car_perceive/main.py b/src/Unmanned_car_perceive/main.py index bfdc8f64a0..3a562b6279 100644 --- a/src/Unmanned_car_perceive/main.py +++ b/src/Unmanned_car_perceive/main.py @@ -259,9 +259,6 @@ def on_tick(self): # 🆕 显示帧率 self.drawer.display_fps(self.fps) - # 🆕 新增：显示摄像头图像 - self.drawer.display_camera() - # 更新观察者视角跟随车辆 self.update_spectator()