Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 31 additions & 1 deletion src/Unmanned_car_security/drawer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
"""绘制摄像头图像(如果需要)"""
# 这里可以添加摄像头图像的绘制逻辑
Expand Down
30 changes: 29 additions & 1 deletion src/Unmanned_car_security/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@

from drawer import PyGameDrawer
from sync_pygame import SyncPyGame
# 导入障碍物检测器
from obstacle_detector import ObstacleDetector


class Main():
Expand Down Expand Up @@ -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()
Expand All @@ -75,6 +83,7 @@ def __init__(self):
# 开始游戏循环
print("▶️ 启动自动驾驶...")
print("📊 车辆速度和位置将显示在屏幕上")
print("🚧 障碍物检测系统已启用")
print("ℹ️ 按ESC键退出程序")
print("=" * 50)

Expand Down Expand Up @@ -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:
Expand All @@ -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)} 个点")
Expand Down Expand Up @@ -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()
Expand Down
76 changes: 76 additions & 0 deletions src/Unmanned_car_security/obstacle_detector.py
Original file line number Diff line number Diff line change
@@ -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) # 红色
Loading