diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py index 0867451b4d..e7c6795370 100644 --- a/src/unmannedcar_MPC/drawer.py +++ b/src/unmannedcar_MPC/drawer.py @@ -1,582 +1,294 @@ +#!/usr/bin/env python3 + import carla import config as Config -import numpy as np import math -import time - - -class PyGameDrawer(): - - def __init__(self, main): - self.main = main - self.pygame = main.game.pygame - self.camera = main.game.camera - self.font_14 = self.pygame.freetype.SysFont('Times New Roman', 14) - - # 速度显示相关字体 - self.speed_font_large = self.pygame.freetype.SysFont('Arial', 36) - self.speed_font_small = self.pygame.freetype.SysFont('Arial', 18) - - # 刹车显示相关 - self.brake_font = self.pygame.freetype.SysFont('Arial', 24) - - # 转向显示相关字体 - self.steer_font = self.pygame.freetype.SysFont('Arial', 20) - - # 通用信息字体 - self.info_font = self.pygame.freetype.SysFont('Arial', 16) - - # 初始化时间 - self.start_time = time.time() - - # draw on the camera perspective - - def __w_locs_2_camera_locs(self, w_locs): - camera_locs = [] - for w_loc in w_locs: - bbox = PyGameDrawer.get_location_bbox(w_loc, self.camera) - if math.isnan(bbox[0, 0]) or math.isnan(bbox[0, 1]): - camera_locs.append((-1, -1)) - camera_locs.append((int(bbox[0, 0]), int(bbox[0, 1]))) - return camera_locs - - def draw_camera_text(self, location, color, text): - x, y = self.__w_locs_2_camera_locs([location])[0] - if x >= 0 and x <= Config.PYGAME_WIDTH and y >= 0 and y <= Config.PYGAME_HEIGHT: - self.font_14.render_to(self.main.surface, (x, y), text, color) - - def draw_camera_circles(self, w_locs, color, radius): - cam_locs = self.__w_locs_2_camera_locs(w_locs) - for cam_loc in cam_locs: - self.pygame.draw.circle( - self.main.surface, color, cam_loc, radius, 1) - - def draw_camera_polygon(self, w_locs, color): - if len(w_locs) < 3: - return - points = self.__w_locs_2_camera_locs(w_locs) - self.pygame.draw.polygon(self.main.surface, color, points, 4) - - def draw_camera_lines(self, color, w_locs, width=1): - cam_locs = self.__w_locs_2_camera_locs(w_locs) - for i in range(len(cam_locs) - 1): - self.__draw_camera_line_safe(color, [cam_locs[i][0], cam_locs[i][1]], [ - cam_locs[i + 1][0], cam_locs[i + 1][1]], width) - - def __draw_camera_line_safe(self, color, pt1, pt2, width=1): - if (pt1[0] >= 0 and pt1[0] <= Config.PYGAME_WIDTH and pt1[1] >= 0 and pt1[1] <= Config.PYGAME_HEIGHT and pt2[ - 0] >= 0 and pt2[0] <= Config.PYGAME_WIDTH and pt2[1] >= 0 and pt2[1] <= Config.PYGAME_HEIGHT): - self.pygame.draw.line(self.main.surface, color, pt1, pt2, width) - - # 绘制点的方法 - def draw_point(self, location, color, radius=3): - """在相机视角下绘制一个点""" - cam_loc = self.__w_locs_2_camera_locs([location])[0] - if cam_loc[0] >= 0 and cam_loc[0] <= Config.PYGAME_WIDTH and cam_loc[1] >= 0 and cam_loc[ - 1] <= Config.PYGAME_HEIGHT: - self.pygame.draw.circle(self.main.surface, color, cam_loc, radius) - - # 显示速度方法 - def display_speed(self, speed_kmh): - """在屏幕右上角显示当前速度""" - # 设置速度显示位置 - pos_x = Config.PYGAME_WIDTH - 180 # 屏幕右侧 - pos_y = 30 # 距离顶部30像素 - - # 根据速度设置颜色 - if speed_kmh < 30: - color = (0, 255, 0) # 绿色 - 低速 - elif speed_kmh < 60: - color = (255, 255, 0) # 黄色 - 中速 - elif speed_kmh < 90: - color = (255, 165, 0) # 橙色 - 中高速 +import numpy as np +from drawer import PyGameDrawer +from sync_pygame import SyncPyGame +from mpc import MPC + + +class Main(): + + def __init__(self): + # setup world + self.client = carla.Client(Config.CARLA_SERVER, 2000) + self.client.set_timeout(10.0) + self.world = self.client.load_world(Config.WORLD_NAME) + self.map = self.world.get_map() + + # spawn ego + ego_spawn_point = self.map.get_spawn_points()[100] + bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] + self.ego = self.world.spawn_actor(bp, ego_spawn_point) + + # init game and drawer + self.game = SyncPyGame(self) + self.drawer = PyGameDrawer(self) + self.mpc = MPC(self.drawer, self.ego) + + # 刹车状态跟踪 + self.is_braking = False + self.brake_history = [] + self.speed_history = [] # 速度历史记录 + self.target_speed_kmh = 40 # 目标速度40km/h + self.brake_force = 0.0 # 当前刹车力度 + self.frame_count = 0 # 帧计数 + self.steer_angle = 0.0 # 转向角度 + self.throttle_value = 0.6 # 油门值 + self.control_mode = "AUTO" # 控制模式 + self.collision_warning = False # 碰撞警告 + self.collision_history = [] # 碰撞警告历史 + + # 驾驶评分系统 + self.driving_score = 100.0 # 初始驾驶评分 + self.score_history = [] # 评分历史记录 + self.score_factors = { + 'speed_stability': 0.0, # 速度稳定性 + 'steering_smoothness': 0.0, # 转向平滑度 + 'brake_usage': 0.0, # 刹车使用情况 + 'path_following': 0.0, # 路径跟踪 + 'safety': 0.0 # 安全性 + } + + # start game loop + self.game.game_loop(self.world, self.on_tick) + + def on_tick(self): + self.frame_count += 1 + + # generate reference path (global frame) + lookahead = 5 + wp = self.map.get_waypoint(self.ego.get_location()) + path = [] + + for _ in range(lookahead): + _wps = wp.next(1) + if len(_wps) == 0: + break + wp = _wps[0] + path.append(wp.transform.location) + + # get forward speed + velocity = self.ego.get_velocity() + speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + + # 计算当前速度(km/h) + current_speed_kmh = speed_m_s * 3.6 # m/s to km/h + + # 记录速度历史(用于显示) + self.speed_history.append(current_speed_kmh) + if len(self.speed_history) > 100: # 保留最近100帧的速度历史 + self.speed_history.pop(0) + + dt = 1 / Config.PYGAME_FPS + + # generate control signal + control = carla.VehicleControl() + + # 智能速度控制逻辑 + if self.frame_count < 100: + # 前100帧:全力加速 + control.throttle = 1.0 # 最大油门 + control.brake = 0.0 + self.brake_force = 0.0 + self.is_braking = False + self.throttle_value = 1.0 + elif self.frame_count < 150: + # 第100-150帧:维持油门,让速度继续上升 + control.throttle = 0.8 + control.brake = 0.0 + self.brake_force = 0.0 + self.is_braking = False + self.throttle_value = 0.8 else: - color = (255, 0, 0) # 红色 - 高速 - - # 显示速度值(大字体) - speed_text = f"{speed_kmh:.1f}" - self.speed_font_large.render_to(self.main.surface, (pos_x, pos_y), speed_text, color) - - # 显示单位(小字体) - unit_text = "km/h" - self.speed_font_small.render_to(self.main.surface, (pos_x + 100, pos_y + 15), unit_text, (200, 200, 200)) - - # 绘制速度条背景 - bar_width = 150 - bar_height = 10 - bar_x = pos_x - bar_y = pos_y + 50 - - # 绘制速度条背景 - bar_bg_rect = self.pygame.Rect(bar_x, bar_y, bar_width, bar_height) - self.pygame.draw.rect(self.main.surface, (50, 50, 50), bar_bg_rect) - - # 绘制速度条填充(根据速度比例) - speed_ratio = min(speed_kmh / 120.0, 1.0) # 假设最大速度120 km/h - bar_filled_width = int(bar_width * speed_ratio) - bar_filled_rect = self.pygame.Rect(bar_x, bar_y, bar_filled_width, bar_height) - self.pygame.draw.rect(self.main.surface, color, bar_filled_rect) - - # 绘制速度条边框 - self.pygame.draw.rect(self.main.surface, (255, 255, 255), bar_bg_rect, 1) - - # 显示刹车状态方法 - def display_brake_status(self, is_braking, brake_history, target_speed, frame_count): - """在屏幕左上角显示刹车状态""" - # 设置显示位置(屏幕左上角) - pos_x = 30 - pos_y = 30 - - # 测试模式:在前200帧强制显示刹车状态,让用户能看到效果 - if frame_count < 200: - # 测试模式:每50帧切换一次状态,演示效果 - test_braking = (frame_count // 50) % 2 == 0 - title_text = f"BRAKE STATUS (TEST MODE) - Target: {target_speed} km/h" - self.brake_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) - - if test_braking: - # 测试刹车状态:红色闪烁 - brake_text = "BRAKING (TEST)" - intensity = 200 + 55 * ((frame_count // 5) % 2) # 闪烁效果 - - # 绘制红色背景框 - bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 180, 40) - self.pygame.draw.rect(self.main.surface, (intensity // 4, 0, 0), bg_rect) - self.pygame.draw.rect(self.main.surface, (intensity, 0, 0), bg_rect, 3) - self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, - (intensity, intensity // 3, intensity // 3)) + # 150帧后:开始速度控制 + speed_error = current_speed_kmh - self.target_speed_kmh + + # 动态调整目标速度 + if current_speed_kmh > 45: # 如果速度能到45以上,提高目标速度 + self.target_speed_kmh = 45 + + # 根据转向角度调整目标速度 + if abs(self.steer_angle) > 0.1: # 如果转向角度较大 + # 转弯时降低目标速度 + adjusted_target = self.target_speed_kmh * (1.0 - abs(self.steer_angle) * 2) + speed_error = current_speed_kmh - adjusted_target else: - # 测试正常状态:绿色 - brake_text = "NORMAL (TEST)" - bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 180, 40) - self.pygame.draw.rect(self.main.surface, (0, 30, 0), bg_rect) - self.pygame.draw.rect(self.main.surface, (0, 180, 0), bg_rect, 2) - self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, (0, 255, 100)) - - # 添加测试模式说明 - self.info_font.render_to(self.main.surface, (pos_x, pos_y + 80), - "Test Mode: Forced display of brake states", (255, 255, 0)) - return - - # 正常模式 - # 检查是否在刹车(使用历史记录创建闪烁效果) - should_flash = False - if len(brake_history) >= 5: - # 如果最近5帧中有3帧在刹车,则显示刹车状态 - recent_brakes = brake_history[-5:] - if sum(recent_brakes) >= 3: - should_flash = True - - # 显示标题和目标速度 - title_text = f"BRAKE STATUS - Target: {target_speed} km/h" - self.brake_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) - - if is_braking or should_flash: - # 刹车状态:红色闪烁 - brake_text = "BRAKING" - - # 闪烁效果:根据时间改变亮度 - intensity = 200 + 55 * ((frame_count // 5) % 2) # 每5帧闪烁一次 - - # 绘制红色背景框 - bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 150, 40) - self.pygame.draw.rect(self.main.surface, (intensity // 4, 0, 0), bg_rect) - - # 绘制红色边框 - self.pygame.draw.rect(self.main.surface, (intensity, 0, 0), bg_rect, 3) - - # 显示"BRAKING"文字 - self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, - (intensity, intensity // 3, intensity // 3)) - - # 添加警告符号 - warning_color = (intensity, intensity, 0) # 黄色警告 - self.pygame.draw.circle(self.main.surface, warning_color, (pos_x + 120, pos_y + 50), 12) - warning_font = self.pygame.freetype.SysFont('Arial', 16) - warning_font.render_to(self.main.surface, (pos_x + 115, pos_y + 44), "!", (0, 0, 0)) + speed_error = current_speed_kmh - self.target_speed_kmh + + if speed_error > 5: # 超过目标速度5km/h时强力刹车 + control.throttle = 0.0 + self.brake_force = min(self.brake_force + 0.1, 1.0) # 快速增加刹车力度 + control.brake = self.brake_force + self.is_braking = True + self.throttle_value = 0.0 + elif speed_error > 2: # 超过目标速度2km/h时轻微刹车 + control.throttle = 0.0 + self.brake_force = min(self.brake_force + 0.05, 0.5) # 中等刹车 + control.brake = self.brake_force + self.is_braking = True + self.throttle_value = 0.0 + elif current_speed_kmh < self.target_speed_kmh - 5: # 低于目标速度时全力加速 + control.throttle = 1.0 + self.brake_force = 0.0 + control.brake = 0.0 + self.is_braking = False + self.throttle_value = 1.0 + elif current_speed_kmh < self.target_speed_kmh - 2: # 接近目标速度但稍低 + control.throttle = 0.6 + self.brake_force = 0.0 + control.brake = 0.0 + self.is_braking = False + self.throttle_value = 0.6 + else: # 接近目标速度时维持 + control.throttle = 0.3 + self.brake_force = max(self.brake_force - 0.02, 0.0) # 逐渐释放刹车 + control.brake = self.brake_force + self.is_braking = self.brake_force > 0.05 # 只有刹车力度大于0.05时才显示刹车状态 + self.throttle_value = 0.3 + + # 记录刹车状态历史(用于闪烁效果) + self.brake_history.append(self.is_braking) + if len(self.brake_history) > 20: # 保持最近20帧的记录 + self.brake_history.pop(0) + + # MPC控制转向 + control.steer = self.mpc.run_step(path, speed_m_s, dt) + self.steer_angle = control.steer # 保存转向角度 + + # 碰撞检测逻辑 + self.check_collision_warning(path, current_speed_kmh, self.steer_angle) + + # 计算驾驶评分 + self.calculate_driving_score(current_speed_kmh, self.steer_angle, self.brake_force, path) + + # 如果检测到碰撞风险,自动减速 + if self.collision_warning: + # 紧急刹车 + control.throttle = 0.0 + control.brake = 0.7 # 中等刹车力度 + self.brake_force = 0.7 + self.is_braking = True + self.throttle_value = 0.0 + print(f"碰撞警告!自动刹车,转向角度: {self.steer_angle:.3f}") + + # apply control signal + self.ego.apply_control(control) + + # 在屏幕上显示所有信息 + self.drawer.display_speed(current_speed_kmh) + self.drawer.display_brake_status(self.is_braking, self.brake_history, self.target_speed_kmh, self.frame_count) + self.drawer.display_speed_history(self.speed_history, self.target_speed_kmh) + self.drawer.display_steering(self.steer_angle) + self.drawer.display_throttle_info(self.throttle_value, self.brake_force) + self.drawer.display_control_mode(self.control_mode) + self.drawer.display_frame_info(self.frame_count, dt) + self.drawer.display_collision_warning(self.collision_warning, self.collision_history) + self.drawer.display_driving_score(self.driving_score, self.score_factors, self.score_history) + + def check_collision_warning(self, path, speed_kmh, steer_angle): + """检测可能的碰撞风险""" + # 基于转向角度和速度的简单碰撞检测 + speed_factor = speed_kmh / 100.0 # 速度越快,风险越高 + steer_factor = abs(steer_angle) # 转向角度越大,风险越高 + + # 计算碰撞风险 + collision_risk = speed_factor * (1.0 + steer_factor * 3) + + # 检查是否超过阈值 + warning_threshold = 0.5 + was_warning = self.collision_warning + self.collision_warning = collision_risk > warning_threshold + + # 记录警告历史 + self.collision_history.append(self.collision_warning) + if len(self.collision_history) > 30: # 保留最近30帧的记录 + self.collision_history.pop(0) + + # 如果状态改变,输出信息 + if self.collision_warning != was_warning: + if self.collision_warning: + print(f"碰撞警告激活!速度: {speed_kmh:.1f} km/h, 转向: {steer_angle:.3f}, 风险: {collision_risk:.2f}") + else: + print("碰撞警告解除") + + def calculate_driving_score(self, current_speed, steer_angle, brake_force, path): + """计算驾驶评分""" + # 1. 速度稳定性评分 (权重30%) + if len(self.speed_history) >= 10: + recent_speeds = self.speed_history[-10:] + speed_variance = np.var(recent_speeds) if len(recent_speeds) > 1 else 0 + # 速度变化越小,分数越高 + speed_stability = max(0, 100 - speed_variance * 5) else: - # 正常状态:绿色 - brake_text = "NORMAL" - - # 绘制绿色背景框 - bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 150, 40) - self.pygame.draw.rect(self.main.surface, (0, 30, 0), bg_rect) - - # 绘制绿色边框 - self.pygame.draw.rect(self.main.surface, (0, 180, 0), bg_rect, 2) - - # 显示"NORMAL"文字 - self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, (0, 255, 100)) - - # 显示速度历史图表 - def display_speed_history(self, speed_history, target_speed): - """在屏幕左下角显示速度历史图表""" - if len(speed_history) < 2: - return - - # 图表位置和大小 - chart_x = 30 - chart_y = Config.PYGAME_HEIGHT - 150 - chart_width = 300 - chart_height = 120 - - # 绘制图表背景 - chart_bg_rect = self.pygame.Rect(chart_x, chart_y, chart_width, chart_height) - self.pygame.draw.rect(self.main.surface, (20, 20, 20), chart_bg_rect) - self.pygame.draw.rect(self.main.surface, (100, 100, 100), chart_bg_rect, 2) - - # 绘制图表标题 - title_font = self.pygame.freetype.SysFont('Arial', 16) - title_font.render_to(self.main.surface, (chart_x + 5, chart_y - 20), "SPEED HISTORY", (200, 200, 200)) - - # 计算速度和目标速度的最小值、最大值 - all_speeds = speed_history + [target_speed] - min_speed = min(all_speeds) - 5 - max_speed = max(all_speeds) + 5 - - # 绘制目标速度线 - if min_speed <= target_speed <= max_speed: - target_y = chart_y + chart_height - int((target_speed - min_speed) / (max_speed - min_speed) * chart_height) - self.pygame.draw.line( - self.main.surface, - (0, 255, 0), # 绿色目标线 - (chart_x, target_y), - (chart_x + chart_width, target_y), - 2 - ) - - # 标注目标速度值 - target_font = self.pygame.freetype.SysFont('Arial', 12) - target_font.render_to(self.main.surface, (chart_x + chart_width + 5, target_y - 10), - f"Target: {target_speed} km/h", (0, 255, 0)) - - # 绘制速度历史曲线 - points = [] - for i, speed in enumerate(speed_history): - if i >= chart_width: # 只显示最近chart_width个数据点 - speed_subset = speed_history[-chart_width:] - break + speed_stability = 80 # 初始分数 - x = chart_x + int(i * chart_width / min(len(speed_history), chart_width)) - y = chart_y + chart_height - int((speed - min_speed) / (max_speed - min_speed) * chart_height) - points.append((x, y)) - - # 连接点成线 - if len(points) > 1: - # 速度线:蓝色 - self.pygame.draw.lines(self.main.surface, (100, 150, 255), False, points, 2) - - # 绘制最后一个点(当前速度) - if points: - last_point = points[-1] - self.pygame.draw.circle(self.main.surface, (255, 255, 255), last_point, 4) - - # 标注当前速度值 - current_speed = speed_history[-1] - speed_font = self.pygame.freetype.SysFont('Arial', 12) - speed_font.render_to( - self.main.surface, - (last_point[0] + 10, last_point[1] - 10), - f"{current_speed:.1f} km/h", - (255, 255, 255) - ) - - # 显示转向角度功能 - def display_steering(self, steer_angle): - """在屏幕右下角显示转向角度""" - # 获取屏幕尺寸 - screen_width = Config.PYGAME_WIDTH - screen_height = Config.PYGAME_HEIGHT - - # 设置在屏幕右下角 - pos_x = screen_width - 220 - pos_y = 120 - - # 将转向角度转换为度数和可视化角度 - angle_degrees = steer_angle * 45 # 假设-1到1对应-45度到45度 - - # 根据转向角度设置颜色 - if abs(angle_degrees) < 5: - color = (0, 255, 0) # 绿色 - 直行或小角度 - elif abs(angle_degrees) < 15: - color = (255, 255, 0) # 黄色 - 中等角度 - else: - color = (255, 100, 0) # 橙色 - 大角度 - - # 显示标题 - title_text = "STEERING ANGLE" - self.steer_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) - - # 显示角度值 - angle_text = f"{angle_degrees:+.1f}°" - self.steer_font.render_to(self.main.surface, (pos_x, pos_y + 30), angle_text, color) - - # 显示原始值 - raw_text = f"Raw: {steer_angle:+.3f}" - self.info_font.render_to(self.main.surface, (pos_x, pos_y + 60), raw_text, (150, 150, 150)) - - # 绘制转向可视化指示器 - indicator_width = 180 - indicator_height = 40 - indicator_x = pos_x - 10 - indicator_y = pos_y + 90 - - # 绘制背景 - indicator_bg = self.pygame.Rect(indicator_x, indicator_y, indicator_width, indicator_height) - self.pygame.draw.rect(self.main.surface, (40, 40, 40), indicator_bg) - self.pygame.draw.rect(self.main.surface, (100, 100, 100), indicator_bg, 2) - - # 绘制中心线 - center_x = indicator_x + indicator_width // 2 - self.pygame.draw.line( - self.main.surface, - (200, 200, 200), - (center_x, indicator_y), - (center_x, indicator_y + indicator_height), - 2 - ) - - # 绘制转向指示器 - indicator_center = center_x + int((indicator_width // 2 - 10) * steer_angle) - indicator_radius = 12 - - # 绘制指示器圆圈 - self.pygame.draw.circle(self.main.surface, color, (indicator_center, indicator_y + indicator_height // 2), - indicator_radius) - - # 绘制方向箭头 - arrow_size = 8 - if steer_angle > 0.01: # 右转 - arrow_points = [ - (indicator_center - arrow_size, indicator_y + indicator_height // 2 - arrow_size), - (indicator_center - arrow_size, indicator_y + indicator_height // 2 + arrow_size), - (indicator_center, indicator_y + indicator_height // 2) - ] - elif steer_angle < -0.01: # 左转 - arrow_points = [ - (indicator_center + arrow_size, indicator_y + indicator_height // 2 - arrow_size), - (indicator_center + arrow_size, indicator_y + indicator_height // 2 + arrow_size), - (indicator_center, indicator_y + indicator_height // 2) - ] - else: # 直行 - arrow_points = [ - (indicator_center - arrow_size, indicator_y + indicator_height // 2), - (indicator_center + arrow_size, indicator_y + indicator_height // 2), - (indicator_center, indicator_y + indicator_height // 2 + arrow_size) - ] - - self.pygame.draw.polygon(self.main.surface, (255, 255, 255), arrow_points) - - # 添加标签 - label_font = self.pygame.freetype.SysFont('Arial', 12) - label_font.render_to(self.main.surface, (indicator_x + 5, indicator_y + indicator_height + 5), "LEFT", - (150, 150, 150)) - label_font.render_to(self.main.surface, - (indicator_x + indicator_width - 40, indicator_y + indicator_height + 5), "RIGHT", - (150, 150, 150)) - label_font.render_to(self.main.surface, (center_x - 20, indicator_y + indicator_height + 5), "CENTER", - (150, 150, 150)) - - # 显示油门和刹车信息 - def display_throttle_info(self, throttle_value, brake_value): - """在屏幕右侧中部显示油门和刹车信息""" - # 设置显示位置 - pos_x = Config.PYGAME_WIDTH - 220 - pos_y = 250 - - # 显示标题 - title_text = "CONTROL INPUTS" - self.steer_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) - - # 油门显示 - throttle_text = f"Throttle: {throttle_value:.2f}" - throttle_color = (0, int(255 * throttle_value), 0) # 绿色,亮度随油门变化 - self.info_font.render_to(self.main.surface, (pos_x, pos_y + 30), throttle_text, throttle_color) - - # 油门条 - throttle_bar_width = 150 - throttle_bar_height = 10 - throttle_bar_x = pos_x - throttle_bar_y = pos_y + 50 - - throttle_bar_bg = self.pygame.Rect(throttle_bar_x, throttle_bar_y, throttle_bar_width, throttle_bar_height) - self.pygame.draw.rect(self.main.surface, (50, 50, 50), throttle_bar_bg) - - throttle_filled_width = int(throttle_bar_width * throttle_value) - throttle_filled_rect = self.pygame.Rect(throttle_bar_x, throttle_bar_y, throttle_filled_width, - throttle_bar_height) - self.pygame.draw.rect(self.main.surface, throttle_color, throttle_filled_rect) - self.pygame.draw.rect(self.main.surface, (255, 255, 255), throttle_bar_bg, 1) - - # 刹车显示 - brake_text = f"Brake: {brake_value:.2f}" - brake_color = (int(255 * brake_value), 0, 0) # 红色,亮度随刹车力度变化 - self.info_font.render_to(self.main.surface, (pos_x, pos_y + 70), brake_text, brake_color) - - # 刹车条 - brake_bar_width = 150 - brake_bar_height = 10 - brake_bar_x = pos_x - brake_bar_y = pos_y + 90 - - brake_bar_bg = self.pygame.Rect(brake_bar_x, brake_bar_y, brake_bar_width, brake_bar_height) - self.pygame.draw.rect(self.main.surface, (50, 50, 50), brake_bar_bg) - - brake_filled_width = int(brake_bar_width * brake_value) - brake_filled_rect = self.pygame.Rect(brake_bar_x, brake_bar_y, brake_filled_width, brake_bar_height) - self.pygame.draw.rect(self.main.surface, brake_color, brake_filled_rect) - self.pygame.draw.rect(self.main.surface, (255, 255, 255), brake_bar_bg, 1) - - # 显示控制模式 - def display_control_mode(self, control_mode): - """在屏幕顶部中央显示控制模式""" - pos_x = Config.PYGAME_WIDTH // 2 - 100 - pos_y = 10 - - if control_mode == "AUTO": - color = (0, 200, 255) # 青色 - elif control_mode == "MANUAL": - color = (255, 200, 0) # 橙色 + # 2. 转向平滑度评分 (权重25%) + # 转向变化越小,分数越高 + if self.frame_count > 1: + steer_variance = abs(steer_angle) * 50 # 转向角度越大,扣分越多 + steering_smoothness = max(0, 100 - steer_variance) else: - color = (255, 255, 255) # 白色 - - # 绘制背景框 - bg_rect = self.pygame.Rect(pos_x - 10, pos_y, 220, 40) - self.pygame.draw.rect(self.main.surface, (20, 20, 20, 128), bg_rect) - self.pygame.draw.rect(self.main.surface, color, bg_rect, 2) - - # 显示文本 - mode_text = f"CONTROL MODE: {control_mode}" - mode_font = self.pygame.freetype.SysFont('Arial', 24) - mode_font.render_to(self.main.surface, (pos_x, pos_y + 10), mode_text, color) - - # 显示帧信息 - def display_frame_info(self, frame_count, dt): - """在屏幕左下角显示帧信息""" - pos_x = 30 - pos_y = Config.PYGAME_HEIGHT - 200 - - # 计算FPS - fps = 1.0 / dt if dt > 0 else 0 - - # 计算运行时间 - current_time = time.time() - elapsed_time = current_time - self.start_time - - # 显示信息 - info_texts = [ - f"Frame: {frame_count}", - f"FPS: {fps:.1f}", - f"Time: {elapsed_time:.1f}s", - f"DT: {dt:.3f}s" - ] - - # 绘制背景 - bg_rect = self.pygame.Rect(pos_x - 10, pos_y - 10, 150, 100) - self.pygame.draw.rect(self.main.surface, (20, 20, 20, 128), bg_rect) - self.pygame.draw.rect(self.main.surface, (100, 100, 100), bg_rect, 1) - - # 显示每行信息 - for i, text in enumerate(info_texts): - self.info_font.render_to(self.main.surface, (pos_x, pos_y + i * 20), text, (200, 200, 200)) - - @staticmethod - def get_location_bbox(location, camera): - bb_cords = np.array([[0, 0, 0, 1]]) - cords_x_y_z = PyGameDrawer.location_to_sensor_cords( - bb_cords, location, camera)[:3, :] - cords_y_minus_z_x = np.concatenate( - [cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) - bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x)) - camera_bbox = np.concatenate( - [bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1) - return camera_bbox - - @staticmethod - def location_to_sensor_cords(cords, location, sensor): - world_cord = PyGameDrawer.location_to_world_cords(cords, location) - sensor_cord = PyGameDrawer._world_to_sensor_cords(world_cord, sensor) - return sensor_cord - - @staticmethod - def location_to_world_cords(cords, location): - bb_transform = carla.Transform(location) - vehicle_world_matrix = PyGameDrawer.get_matrix(bb_transform) - world_cords = np.dot(vehicle_world_matrix, np.transpose(cords)) - return world_cords - - @staticmethod - def _create_vehicle_bbox_points(vehicle): - """ - Returns 3D bounding box for a vehicle. - """ - cords = np.zeros((8, 4)) - extent = vehicle.bounding_box.extent - cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1]) - cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1]) - cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1]) - cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1]) - cords[4, :] = np.array([extent.x, extent.y, extent.z, 1]) - cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1]) - cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1]) - cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1]) - return cords - - @staticmethod - def _vehicle_to_sensor_cords(cords, vehicle, sensor): - """ - Transforms coordinates of a vehicle bounding box to sensor. - """ - world_cord = PyGameDrawer._vehicle_to_world_cords(cords, vehicle) - sensor_cord = PyGameDrawer._world_to_sensor_cords(world_cord, sensor) - return sensor_cord - - @staticmethod - def _vehicle_to_world_cords(cords, vehicle): - """ - Transforms coordinates of a vehicle bounding box to world. - """ - bb_transform = carla.Transform(vehicle.bounding_box.location) - bb_vehicle_matrix = PyGameDrawer.get_matrix(bb_transform) - vehicle_world_matrix = PyGameDrawer.get_matrix(vehicle.get_transform()) - bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix) - world_cords = np.dot(bb_world_matrix, np.transpose(cords)) - return world_cords - - @staticmethod - def _world_to_sensor_cords(cords, sensor): - """ - Transforms world coordinates to sensor. - """ - sensor_world_matrix = PyGameDrawer.get_matrix(sensor.get_transform()) - world_sensor_matrix = np.linalg.inv(sensor_world_matrix) - sensor_cords = np.dot(world_sensor_matrix, cords) - return sensor_cords - - @staticmethod - def get_matrix(transform): - """ - Creates matrix from carla transform. - """ - rotation = transform.rotation - location = transform.location - c_y = np.cos(np.radians(rotation.yaw)) - s_y = np.sin(np.radians(rotation.yaw)) - c_r = np.cos(np.radians(rotation.roll)) - s_r = np.sin(np.radians(rotation.roll)) - c_p = np.cos(np.radians(rotation.pitch)) - s_p = np.sin(np.radians(rotation.pitch)) - matrix = np.matrix(np.identity(4)) - matrix[0, 3] = location.x - matrix[1, 3] = location.y - matrix[2, 3] = location.z - matrix[0, 0] = c_p * c_y - matrix[0, 1] = c_y * s_p * s_r - s_y * c_r - matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r - matrix[1, 0] = s_y * c_p - matrix[1, 1] = s_y * s_p * s_r + c_y * c_r - matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r - matrix[2, 0] = s_p - matrix[2, 1] = -c_p * s_r - matrix[2, 2] = c_p * c_r - return matrix \ No newline at end of file + steering_smoothness = 85 + + # 3. 刹车使用评分 (权重20%) + # 刹车使用越少,分数越高 + brake_usage = max(0, 100 - brake_force * 120) # 刹车力度越大,扣分越多 + + # 4. 路径跟踪评分 (权重15%) + # 这里简化处理,使用转向角度作为路径跟踪的间接指标 + path_following = max(0, 100 - abs(steer_angle) * 40) + + # 5. 安全性评分 (权重10%) + # 安全事件越少,分数越高 + safety_penalty = 0 + if self.collision_warning: + safety_penalty += 30 # 碰撞警告扣分 + if brake_force > 0.5: + safety_penalty += 20 # 紧急刹车扣分 + safety = max(0, 100 - safety_penalty) + + # 保存各项评分因子 + self.score_factors['speed_stability'] = speed_stability + self.score_factors['steering_smoothness'] = steering_smoothness + self.score_factors['brake_usage'] = brake_usage + self.score_factors['path_following'] = path_following + self.score_factors['safety'] = safety + + # 计算综合评分 (加权平均) + weights = { + 'speed_stability': 0.30, + 'steering_smoothness': 0.25, + 'brake_usage': 0.20, + 'path_following': 0.15, + 'safety': 0.10 + } + + total_score = 0 + for factor, weight in weights.items(): + total_score += self.score_factors[factor] * weight + + # 应用平滑更新 (避免分数突变) + self.driving_score = 0.7 * self.driving_score + 0.3 * total_score + + # 记录评分历史 + self.score_history.append(self.driving_score) + if len(self.score_history) > 200: # 保留最近200帧的评分历史 + self.score_history.pop(0) + + # 每100帧输出一次评分信息 + if self.frame_count % 100 == 0: + print(f"\n=== 驾驶评分报告 (帧 {self.frame_count}) ===") + print(f"综合评分: {self.driving_score:.1f}/100") + print(f"速度稳定性: {speed_stability:.1f}") + print(f"转向平滑度: {steering_smoothness:.1f}") + print(f"刹车使用: {brake_usage:.1f}") + print(f"路径跟踪: {path_following:.1f}") + print(f"安全性: {safety:.1f}") + print("=" * 40) + + +if __name__ == '__main__': + Main() \ No newline at end of file diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index ade69af70b..e7c6795370 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -3,6 +3,7 @@ import carla import config as Config import math +import numpy as np from drawer import PyGameDrawer from sync_pygame import SyncPyGame from mpc import MPC @@ -37,6 +38,19 @@ def __init__(self): self.steer_angle = 0.0 # 转向角度 self.throttle_value = 0.6 # 油门值 self.control_mode = "AUTO" # 控制模式 + self.collision_warning = False # 碰撞警告 + self.collision_history = [] # 碰撞警告历史 + + # 驾驶评分系统 + self.driving_score = 100.0 # 初始驾驶评分 + self.score_history = [] # 评分历史记录 + self.score_factors = { + 'speed_stability': 0.0, # 速度稳定性 + 'steering_smoothness': 0.0, # 转向平滑度 + 'brake_usage': 0.0, # 刹车使用情况 + 'path_following': 0.0, # 路径跟踪 + 'safety': 0.0 # 安全性 + } # start game loop self.game.game_loop(self.world, self.on_tick) @@ -96,6 +110,14 @@ def on_tick(self): if current_speed_kmh > 45: # 如果速度能到45以上,提高目标速度 self.target_speed_kmh = 45 + # 根据转向角度调整目标速度 + if abs(self.steer_angle) > 0.1: # 如果转向角度较大 + # 转弯时降低目标速度 + adjusted_target = self.target_speed_kmh * (1.0 - abs(self.steer_angle) * 2) + speed_error = current_speed_kmh - adjusted_target + else: + speed_error = current_speed_kmh - self.target_speed_kmh + if speed_error > 5: # 超过目标速度5km/h时强力刹车 control.throttle = 0.0 self.brake_force = min(self.brake_force + 0.1, 1.0) # 快速增加刹车力度 @@ -136,6 +158,22 @@ def on_tick(self): control.steer = self.mpc.run_step(path, speed_m_s, dt) self.steer_angle = control.steer # 保存转向角度 + # 碰撞检测逻辑 + self.check_collision_warning(path, current_speed_kmh, self.steer_angle) + + # 计算驾驶评分 + self.calculate_driving_score(current_speed_kmh, self.steer_angle, self.brake_force, path) + + # 如果检测到碰撞风险,自动减速 + if self.collision_warning: + # 紧急刹车 + control.throttle = 0.0 + control.brake = 0.7 # 中等刹车力度 + self.brake_force = 0.7 + self.is_braking = True + self.throttle_value = 0.0 + print(f"碰撞警告!自动刹车,转向角度: {self.steer_angle:.3f}") + # apply control signal self.ego.apply_control(control) @@ -147,6 +185,109 @@ def on_tick(self): self.drawer.display_throttle_info(self.throttle_value, self.brake_force) self.drawer.display_control_mode(self.control_mode) self.drawer.display_frame_info(self.frame_count, dt) + self.drawer.display_collision_warning(self.collision_warning, self.collision_history) + self.drawer.display_driving_score(self.driving_score, self.score_factors, self.score_history) + + def check_collision_warning(self, path, speed_kmh, steer_angle): + """检测可能的碰撞风险""" + # 基于转向角度和速度的简单碰撞检测 + speed_factor = speed_kmh / 100.0 # 速度越快,风险越高 + steer_factor = abs(steer_angle) # 转向角度越大,风险越高 + + # 计算碰撞风险 + collision_risk = speed_factor * (1.0 + steer_factor * 3) + + # 检查是否超过阈值 + warning_threshold = 0.5 + was_warning = self.collision_warning + self.collision_warning = collision_risk > warning_threshold + + # 记录警告历史 + self.collision_history.append(self.collision_warning) + if len(self.collision_history) > 30: # 保留最近30帧的记录 + self.collision_history.pop(0) + + # 如果状态改变,输出信息 + if self.collision_warning != was_warning: + if self.collision_warning: + print(f"碰撞警告激活!速度: {speed_kmh:.1f} km/h, 转向: {steer_angle:.3f}, 风险: {collision_risk:.2f}") + else: + print("碰撞警告解除") + + def calculate_driving_score(self, current_speed, steer_angle, brake_force, path): + """计算驾驶评分""" + # 1. 速度稳定性评分 (权重30%) + if len(self.speed_history) >= 10: + recent_speeds = self.speed_history[-10:] + speed_variance = np.var(recent_speeds) if len(recent_speeds) > 1 else 0 + # 速度变化越小,分数越高 + speed_stability = max(0, 100 - speed_variance * 5) + else: + speed_stability = 80 # 初始分数 + + # 2. 转向平滑度评分 (权重25%) + # 转向变化越小,分数越高 + if self.frame_count > 1: + steer_variance = abs(steer_angle) * 50 # 转向角度越大,扣分越多 + steering_smoothness = max(0, 100 - steer_variance) + else: + steering_smoothness = 85 + + # 3. 刹车使用评分 (权重20%) + # 刹车使用越少,分数越高 + brake_usage = max(0, 100 - brake_force * 120) # 刹车力度越大,扣分越多 + + # 4. 路径跟踪评分 (权重15%) + # 这里简化处理,使用转向角度作为路径跟踪的间接指标 + path_following = max(0, 100 - abs(steer_angle) * 40) + + # 5. 安全性评分 (权重10%) + # 安全事件越少,分数越高 + safety_penalty = 0 + if self.collision_warning: + safety_penalty += 30 # 碰撞警告扣分 + if brake_force > 0.5: + safety_penalty += 20 # 紧急刹车扣分 + safety = max(0, 100 - safety_penalty) + + # 保存各项评分因子 + self.score_factors['speed_stability'] = speed_stability + self.score_factors['steering_smoothness'] = steering_smoothness + self.score_factors['brake_usage'] = brake_usage + self.score_factors['path_following'] = path_following + self.score_factors['safety'] = safety + + # 计算综合评分 (加权平均) + weights = { + 'speed_stability': 0.30, + 'steering_smoothness': 0.25, + 'brake_usage': 0.20, + 'path_following': 0.15, + 'safety': 0.10 + } + + total_score = 0 + for factor, weight in weights.items(): + total_score += self.score_factors[factor] * weight + + # 应用平滑更新 (避免分数突变) + self.driving_score = 0.7 * self.driving_score + 0.3 * total_score + + # 记录评分历史 + self.score_history.append(self.driving_score) + if len(self.score_history) > 200: # 保留最近200帧的评分历史 + self.score_history.pop(0) + + # 每100帧输出一次评分信息 + if self.frame_count % 100 == 0: + print(f"\n=== 驾驶评分报告 (帧 {self.frame_count}) ===") + print(f"综合评分: {self.driving_score:.1f}/100") + print(f"速度稳定性: {speed_stability:.1f}") + print(f"转向平滑度: {steering_smoothness:.1f}") + print(f"刹车使用: {brake_usage:.1f}") + print(f"路径跟踪: {path_following:.1f}") + print(f"安全性: {safety:.1f}") + print("=" * 40) if __name__ == '__main__':