From 20e276a8b59910f31457b27368fd297e97f4c74b Mon Sep 17 00:00:00 2001 From: 183899 Date: Mon, 1 Dec 2025 11:44:13 +0800 Subject: [PATCH 01/14] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/driverless/main.py | 60 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 src/driverless/main.py diff --git a/src/driverless/main.py b/src/driverless/main.py new file mode 100644 index 0000000000..6a8726fa54 --- /dev/null +++ b/src/driverless/main.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 + +import carla +import config as Config +import math +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) + + # start game loop + self.game.game_loop(self.world, self.on_tick) + + def on_tick(self): + # 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) + dt = 1 / Config.PYGAME_FPS + + # generate control signal + control = carla.VehicleControl() + control.throttle = 0.6 + control.steer = self.mpc.run_step(path, speed_m_s, dt) + + # apply control signal + self.ego.apply_control(control) + +if __name__ == '__main__': + Main() \ No newline at end of file From 4a2ccc8b67a058c5855cf350d35f1aa73eaaaa34 Mon Sep 17 00:00:00 2001 From: 183899 Date: Mon, 8 Dec 2025 09:17:03 +0800 Subject: [PATCH 02/14] =?UTF-8?q?=E5=B0=86=E5=8E=9F=E6=A8=A1=E5=9D=97?= =?UTF-8?q?=E5=90=8D=E6=9B=B4=E6=94=B9=E4=B8=BA=E6=9B=B4=E5=8A=A0=E8=AF=A6?= =?UTF-8?q?=E7=BB=86=E7=9A=84=E5=90=8D=E5=AD=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/{driverless => unmannedcar_MPC}/RADME.md | 0 src/{driverless => unmannedcar_MPC}/main.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{driverless => unmannedcar_MPC}/RADME.md (100%) rename src/{driverless => unmannedcar_MPC}/main.py (100%) diff --git a/src/driverless/RADME.md b/src/unmannedcar_MPC/RADME.md similarity index 100% rename from src/driverless/RADME.md rename to src/unmannedcar_MPC/RADME.md diff --git a/src/driverless/main.py b/src/unmannedcar_MPC/main.py similarity index 100% rename from src/driverless/main.py rename to src/unmannedcar_MPC/main.py From 2b3e4b21265f434d5acdf8c6c01189b3192a20fc Mon Sep 17 00:00:00 2001 From: 183899 Date: Mon, 15 Dec 2025 10:03:16 +0800 Subject: [PATCH 03/14] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=99=A8=E5=8A=9F=E8=83=BD=EF=BC=8C=E5=A2=9E=E6=B7=BB?= =?UTF-8?q?=E4=BA=86=E9=80=9F=E5=BA=A6=E6=98=BE=E7=A4=BA=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/main.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index 6a8726fa54..9877ff1bbc 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +##!/usr/bin/env python3 import carla import config as Config @@ -46,6 +46,10 @@ def on_tick(self): # 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 + dt = 1 / Config.PYGAME_FPS # generate control signal @@ -56,5 +60,8 @@ def on_tick(self): # apply control signal self.ego.apply_control(control) + # 在屏幕上显示速度 + self.drawer.display_speed(current_speed_kmh) + if __name__ == '__main__': Main() \ No newline at end of file From 1f64807dc4c5273edd75e999f029ee38d0a9297c Mon Sep 17 00:00:00 2001 From: 183899 Date: Tue, 16 Dec 2025 08:29:12 +0800 Subject: [PATCH 04/14] =?UTF-8?q?=E8=A1=A5=E5=85=85=E5=B9=B6=E5=9C=A8?= =?UTF-8?q?=E5=8E=9F=E6=9C=89=E7=9A=84=20drawer.py=20=E4=B8=AD=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E4=BA=86=20draw=5Fpoint=20=E6=96=B9=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/drawer.py | 209 ++++++++++++++++++++++++++++++++++ 1 file changed, 209 insertions(+) create mode 100644 src/unmannedcar_MPC/drawer.py diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py new file mode 100644 index 0000000000..6058fcbf72 --- /dev/null +++ b/src/unmannedcar_MPC/drawer.py @@ -0,0 +1,209 @@ +import carla +import config as Config +import numpy as np +import math + + +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) + + # 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) + + # 新增：绘制点的方法（用于修复AttributeError） + 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) # 橙色 - 中高速 + 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) + + @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 From 417561d6a5a20999861195c0248fa5ddc9caab33 Mon Sep 17 00:00:00 2001 From: 183899 Date: Thu, 18 Dec 2025 23:13:52 +0800 Subject: [PATCH 05/14] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=96=B0=E7=9A=84?= =?UTF-8?q?=E8=BD=AC=E5=BC=AF=E6=84=9F=E7=9F=A5=E6=A8=A1=E5=9D=97=EF=BC=8C?= =?UTF-8?q?=E4=BF=AE=E6=94=B9main=E5=87=BD=E6=95=B0=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=E8=BD=AC=E5=BC=AF=E6=97=B6=E8=87=AA=E4=B8=BB=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E5=88=B9=E8=BD=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/config.py | 54 +++++++ src/unmannedcar_MPC/main.py | 266 ++++++++++++++++++++++++++-------- 2 files changed, 258 insertions(+), 62 deletions(-) create mode 100644 src/unmannedcar_MPC/config.py diff --git a/src/unmannedcar_MPC/config.py b/src/unmannedcar_MPC/config.py new file mode 100644 index 0000000000..03a31ab614 --- /dev/null +++ b/src/unmannedcar_MPC/config.py @@ -0,0 +1,54 @@ +# config.py +# CARLA 连接配置 +CARLA_SERVER = 'localhost' # CARLA 服务器地址 +WORLD_NAME = 'Town05' # 默认地图名称 + +# PyGame 显示配置 +PYGAME_WIDTH = 800 # 窗口宽度 +PYGAME_HEIGHT = 600 # 窗口高度 +PYGAME_FPS = 20 # 帧率 +PYGAME_FOV = 90 # 相机视场角（Field of View） + +# 相机配置 +CAMERA_CONFIG = { + 'image_size_x': PYGAME_WIDTH, + 'image_size_y': PYGAME_HEIGHT, + 'fov': PYGAME_FOV, +} + +# MPC 控制参数 +MPC_CONFIG = { + 'N': 10, # 预测时域 + 'DT': 0.1, # 时间步长 + 'L_F': 1.5, # 前轴到质心距离 + 'L_R': 1.5, # 后轴到质心距离 + 'MAX_STEER': 0.6, # 最大转向角 + 'MAX_DSTEER': 0.1, # 最大转向角变化率 +} + +# 弯道减速参数 +CURVE_CONTROL = { + 'MAX_SPEED_KMH': 60, # 直道最大速度（km/h） + 'MIN_SPEED_KMH': 20, # 弯道最小速度（km/h） + 'DEFAULT_THROTTLE': 0.6, # 默认油门值 + 'CURVE_THRESHOLD': 0.3, # 弯道阈值 + 'LOOKAHEAD_DISTANCE': 8, # 前瞻距离（点数量） + 'SMOOTHING_ALPHA': 0.3, # 平滑系数 +} + +# 感知减速参数（如果还需要） +PERCEPTION = { + 'SAFETY_DISTANCE': 10.0, # 安全距离（米） + 'EMERGENCY_DISTANCE': 5.0, # 紧急刹车距离（米） + 'DETECTION_RANGE': 30.0, # 检测范围（米） + 'DETECTION_ANGLE': 60, # 检测角度（度） + 'DEFAULT_THROTTLE': 0.6, # 默认油门值 +} + +# 车辆参数 +VEHICLE_CONFIG = { + 'MAX_SPEED': 70, # 最大速度 km/h + 'MAX_THROTTLE': 0.8, # 最大油门 + 'MAX_BRAKE': 1.0, # 最大刹车 + 'MAX_STEER': 0.7, # 最大转向 +} \ No newline at end of file diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index 9877ff1bbc..6058fcbf72 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -1,67 +1,209 @@ -##!/usr/bin/env python3 - import carla import config as Config +import numpy as np import math -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) - - # start game loop - self.game.game_loop(self.world, self.on_tick) - - def on_tick(self): - # 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 - - dt = 1 / Config.PYGAME_FPS - - # generate control signal - control = carla.VehicleControl() - control.throttle = 0.6 - control.steer = self.mpc.run_step(path, speed_m_s, dt) - - # apply control signal - self.ego.apply_control(control) - # 在屏幕上显示速度 - self.drawer.display_speed(current_speed_kmh) -if __name__ == '__main__': - Main() \ No newline at end of file +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) + + # 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) + + # 新增：绘制点的方法（用于修复AttributeError） + 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) # 橙色 - 中高速 + 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) + + @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 From 532b4ab92f583e955bb42c4cfdd1dc880a57c008 Mon Sep 17 00:00:00 2001 From: 183899 Date: Fri, 19 Dec 2025 18:57:02 +0800 Subject: [PATCH 06/14] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E9=A2=84=E6=B5=8B?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=EF=BC=8C=E6=8F=90=E5=89=8D=E9=A2=84=E6=B5=8B?= =?UTF-8?q?=E8=BD=AC=E5=BC=AF=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/drawer.py | 381 +++++++++++++++++++++++++++++++++- src/unmannedcar_MPC/main.py | 354 +++++++++++++------------------ 2 files changed, 526 insertions(+), 209 deletions(-) diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py index 6058fcbf72..0867451b4d 100644 --- a/src/unmannedcar_MPC/drawer.py +++ b/src/unmannedcar_MPC/drawer.py @@ -2,6 +2,7 @@ import config as Config import numpy as np import math +import time class PyGameDrawer(): @@ -12,10 +13,22 @@ def __init__(self, main): 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): @@ -55,7 +68,7 @@ def __draw_camera_line_safe(self, color, pt1, pt2, width=1): 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) - # 新增：绘制点的方法（用于修复AttributeError） + # 绘制点的方法 def draw_point(self, location, color, radius=3): """在相机视角下绘制一个点""" cam_loc = self.__w_locs_2_camera_locs([location])[0] @@ -63,7 +76,7 @@ def draw_point(self, location, color, radius=3): 1] <= Config.PYGAME_HEIGHT: self.pygame.draw.circle(self.main.surface, color, cam_loc, radius) - # 新增：显示速度方法 + # 显示速度方法 def display_speed(self, speed_kmh): """在屏幕右上角显示当前速度""" # 设置速度显示位置 @@ -88,7 +101,7 @@ def display_speed(self, speed_kmh): 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 @@ -107,6 +120,366 @@ def display_speed(self, speed_kmh): # 绘制速度条边框 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)) + 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)) + 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 + + 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) # 橙色 + 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]]) diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index 6058fcbf72..ade69af70b 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -1,209 +1,153 @@ +#!/usr/bin/env python3 + import carla import config as Config -import numpy as np import math - - -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) - - # 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) - - # 新增：绘制点的方法（用于修复AttributeError） - 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) # 橙色 - 中高速 +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" # 控制模式 + + # 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) - - @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 + # 150帧后：开始速度控制 + speed_error = current_speed_kmh - self.target_speed_kmh + + # 动态调整目标速度 + if current_speed_kmh > 45: # 如果速度能到45以上，提高目标速度 + self.target_speed_kmh = 45 + + 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 # 保存转向角度 + + # 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) + + +if __name__ == '__main__': + Main() \ No newline at end of file From 85832accb6cdb4edb64502547262d3a2239c57ac Mon Sep 17 00:00:00 2001 From: 183899 Date: Sat, 20 Dec 2025 16:32:27 +0800 Subject: [PATCH 07/14] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86REDME.py?= =?UTF-8?q?=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/RADME.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/unmannedcar_MPC/RADME.md b/src/unmannedcar_MPC/RADME.md index d82241b591..3f7d1cc7a7 100644 --- a/src/unmannedcar_MPC/RADME.md +++ b/src/unmannedcar_MPC/RADME.md @@ -1 +1,14 @@ -我的选题是无人车控制中的基于mpc的车辆转向控制器。 +这是一个基于CARLA模拟器的主动式车道保持辅助系统项目，它就像一个虚拟的智能驾驶员，能够通过摄像头观察道路情况，自动控制方向盘使车辆稳定行驶在车道中心。 + +整个系统的运作始于一个摄像头，它安装在虚拟车辆的前方，持续捕捉前方的道路图像。这些彩色图像被送入一个视觉处理程序，首先将图像从常见的RGB色彩空间转换到HLS空间，这是因为车道线在这种色彩模式下更容易被识别。程序会过滤出白色和黄色的像素，这些能就是车道线。接着，通过一种叫做透视变换的技术，把图像转换成鸟瞰图，仿佛我们从天空俯视道路一样，这样能更准确地判断车道线的弯曲程度和车辆的位置。然后，系统采用滑动窗口的方法，从图像底部开始，像爬楼梯一样一层层向上寻找属于车道线的像素点，再用一条光滑的曲线去拟合这些点，从而得到两条清晰的车道边界。最后，计算出车辆中心点与这两条边界中心线的横向距离，这个距离就是车辆偏离车道中心的误差。 + +环境安装：python -m venv venv + +. venv/bin/activate + +pip install --upgrade pip + +pip install -r requirements.txt + +结果：生成一辆小车，按照预定轨迹提前改变路径，实现控制预测与转弯。 + From 94c10493df91748230b45264791e20cafae935cc Mon Sep 17 00:00:00 2001 From: 183899 Date: Sun, 21 Dec 2025 15:51:05 +0800 Subject: [PATCH 08/14] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86config.py?= =?UTF-8?q?=EF=BC=8C=E8=A7=A3=E5=86=B3=E4=BA=86=E5=8E=9F=E5=85=88=E5=B0=8F?= =?UTF-8?q?=E8=BD=A6=E8=BD=AC=E5=BC=AF=E5=8D=8A=E5=BE=84=E8=BF=87=E5=B0=8F?= =?UTF-8?q?=E5=AF=BC=E8=87=B4=E7=9A=84=E6=92=9E=E5=A2=99=E6=83=85=E5=86=B5?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/config.py | 83 +++++++++++++++++------------------ 1 file changed, 41 insertions(+), 42 deletions(-) diff --git a/src/unmannedcar_MPC/config.py b/src/unmannedcar_MPC/config.py index 03a31ab614..af373eb263 100644 --- a/src/unmannedcar_MPC/config.py +++ b/src/unmannedcar_MPC/config.py @@ -1,54 +1,53 @@ -# config.py -# CARLA 连接配置 -CARLA_SERVER = 'localhost' # CARLA 服务器地址 -WORLD_NAME = 'Town05' # 默认地图名称 +""" +自动驾驶系统配置文件 +""" -# PyGame 显示配置 -PYGAME_WIDTH = 800 # 窗口宽度 -PYGAME_HEIGHT = 600 # 窗口高度 -PYGAME_FPS = 20 # 帧率 -PYGAME_FOV = 90 # 相机视场角（Field of View） +# Carla连接配置 +CARLA_SERVER = 'localhost' +CARLA_PORT = 2000 +WORLD_NAME = 'Town05' -# 相机配置 -CAMERA_CONFIG = { - 'image_size_x': PYGAME_WIDTH, - 'image_size_y': PYGAME_HEIGHT, - 'fov': PYGAME_FOV, +# 显示配置 +PYGAME_WIDTH = 1280 +PYGAME_HEIGHT = 720 +PYGAME_FPS = 30 +PYGAME_FOV = 90 + +# 车辆控制配置 +VEHICLE_CONFIG = { + 'MAX_SPEED_KMH': 60, # 最大速度 + 'MIN_SPEED_KMH': 20, # 最小速度 + 'DEFAULT_THROTTLE': 0.6, # 默认油门 + 'MAX_STEER': 0.7, # 最大转向 + 'MAX_BRAKE': 1.0, # 最大刹车 } -# MPC 控制参数 +# MPC控制器配置 MPC_CONFIG = { - 'N': 10, # 预测时域 - 'DT': 0.1, # 时间步长 - 'L_F': 1.5, # 前轴到质心距离 - 'L_R': 1.5, # 后轴到质心距离 - 'MAX_STEER': 0.6, # 最大转向角 - 'MAX_DSTEER': 0.1, # 最大转向角变化率 + 'PREDICTION_HORIZON': 10, # 预测时域 + 'TIME_STEP': 0.1, # 时间步长 + 'WHEELBASE': 3.0, # 轴距 + 'MAX_STEER_RATE': 0.1, # 最大转向变化率 } -# 弯道减速参数 -CURVE_CONTROL = { - 'MAX_SPEED_KMH': 60, # 直道最大速度（km/h） - 'MIN_SPEED_KMH': 20, # 弯道最小速度（km/h） - 'DEFAULT_THROTTLE': 0.6, # 默认油门值 - 'CURVE_THRESHOLD': 0.3, # 弯道阈值 - 'LOOKAHEAD_DISTANCE': 8, # 前瞻距离（点数量） - 'SMOOTHING_ALPHA': 0.3, # 平滑系数 +# 弯道控制配置 +CURVE_CONFIG = { + 'LOOKAHEAD_POINTS': 8, # 前瞻点数 + 'CURVATURE_THRESHOLD': 0.3, # 弯道阈值 + 'SMOOTHING_FACTOR': 0.3, # 平滑系数 } -# 感知减速参数（如果还需要） -PERCEPTION = { - 'SAFETY_DISTANCE': 10.0, # 安全距离（米） - 'EMERGENCY_DISTANCE': 5.0, # 紧急刹车距离（米） - 'DETECTION_RANGE': 30.0, # 检测范围（米） - 'DETECTION_ANGLE': 60, # 检测角度（度） - 'DEFAULT_THROTTLE': 0.6, # 默认油门值 +# 车道保持配置 +LANE_KEEP_CONFIG = { + 'ACTIVE': True, # 是否激活 + 'KP': 0.05, # 比例系数 + 'MAX_CORRECTION': 0.15, # 最大修正 + 'WARNING_THRESHOLD': 1.0, # 警告阈值 } -# 车辆参数 -VEHICLE_CONFIG = { - 'MAX_SPEED': 70, # 最大速度 km/h - 'MAX_THROTTLE': 0.8, # 最大油门 - 'MAX_BRAKE': 1.0, # 最大刹车 - 'MAX_STEER': 0.7, # 最大转向 +# 性能配置 +PERFORMANCE_CONFIG = { + 'ENABLE_FPS_DISPLAY': True, # 显示FPS + 'LOG_INTERVAL': 100, # 日志间隔 + 'DEBUG_MODE': False, # 调试模式 } \ No newline at end of file From c9d9d498839977570d631e9473c135b5dc28977a Mon Sep 17 00:00:00 2001 From: 183899 Date: Sun, 21 Dec 2025 21:04:22 +0800 Subject: [PATCH 09/14] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E6=B5=8B=E8=AF=84?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=EF=BC=8C=E5=B0=86=E8=BD=A6=E8=BE=86=E7=8A=B6?= =?UTF-8?q?=E6=80=81=E5=AE=9E=E6=97=B6=E6=98=BE=E7=A4=BA=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/drawer.py | 860 +++++++++++----------------------- src/unmannedcar_MPC/main.py | 141 ++++++ 2 files changed, 427 insertions(+), 574 deletions(-) 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__': From 57d483a3f242e2ab6cdd7389c6269b8404787ef4 Mon Sep 17 00:00:00 2001 From: 183899 Date: Mon, 22 Dec 2025 10:37:37 +0800 Subject: [PATCH 10/14] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E4=BB=A3=E7=A0=81=EF=BC=8C=E6=B7=BB=E5=8A=A0=E4=BA=86?= =?UTF-8?q?=E8=88=AA=E7=82=B9=E5=AF=BC=E8=88=AA=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/drawer.py | 1171 +++++++++++++++++++++++++-------- src/unmannedcar_MPC/main.py | 64 ++ 2 files changed, 952 insertions(+), 283 deletions(-) diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py index e7c6795370..e5c118a84e 100644 --- a/src/unmannedcar_MPC/drawer.py +++ b/src/unmannedcar_MPC/drawer.py @@ -1,294 +1,899 @@ -#!/usr/bin/env python3 - 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 - - -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 +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() + self.frame_count = 0 # 帧计数器 + + # 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): + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + if (pt1[0] >= 0 and pt1[0] <= screen_width and pt1[1] >= 0 and pt1[1] <= screen_height and + pt2[0] >= 0 and pt2[0] <= screen_width and pt2[1] >= 0 and pt2[1] <= screen_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] + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + if cam_loc[0] >= 0 and cam_loc[0] <= screen_width and cam_loc[1] >= 0 and cam_loc[1] <= screen_height: + self.pygame.draw.circle(self.main.surface, color, cam_loc, radius) + + # 显示速度方法 + def display_speed(self, speed_kmh): + """在屏幕右上角显示当前速度""" + # 设置速度显示位置 + screen_width = Config.PYGAME_WIDTH + pos_x = screen_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) # 橙色 - 中高速 else: - # 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: - 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}") + 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): + """在屏幕左上角显示刹车状态""" + self.frame_count = 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)) 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) + # 测试正常状态：绿色 + 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)) else: - speed_stability = 80 # 初始分数 + # 正常状态：绿色 + 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 + + # 图表位置和大小 + screen_height = Config.PYGAME_HEIGHT + chart_x = 30 + chart_y = screen_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 - # 2. 转向平滑度评分 (权重25%) - # 转向变化越小，分数越高 - if self.frame_count > 1: - steer_variance = abs(steer_angle) * 50 # 转向角度越大，扣分越多 - steering_smoothness = max(0, 100 - steer_variance) + 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): + """在屏幕右侧中部显示油门和刹车信息""" + # 设置显示位置 + screen_width = Config.PYGAME_WIDTH + pos_x = screen_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): + """在屏幕顶部中央显示控制模式""" + screen_width = Config.PYGAME_WIDTH + pos_x = screen_width // 2 - 100 + pos_y = 10 + + if control_mode == "AUTO": + color = (0, 200, 255) # 青色 + elif control_mode == "MANUAL": + color = (255, 200, 0) # 橙色 + 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): + """在屏幕左下角显示帧信息""" + screen_height = Config.PYGAME_HEIGHT + pos_x = 30 + pos_y = screen_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)) + + # 显示碰撞警告 + def display_collision_warning(self, collision_warning, collision_history): + """在屏幕中央上方显示碰撞警告""" + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 设置显示位置（屏幕中央上方） + pos_x = screen_width // 2 - 150 + pos_y = 150 + + # 检查是否需要显示警告（使用历史记录减少闪烁） + should_warn = False + if len(collision_history) >= 10: + # 如果最近10帧中有7帧检测到碰撞风险，则显示警告 + recent_warnings = collision_history[-10:] + if sum(recent_warnings) >= 7: + should_warn = True + + if collision_warning or should_warn: + # 碰撞警告：红色闪烁 + warning_text = "COLLISION WARNING!" + + # 闪烁效果 + intensity = 200 + 55 * ((self.frame_count // 3) % 2) # 快速闪烁 + + # 绘制警告背景 + bg_rect = self.pygame.Rect(pos_x - 20, pos_y - 10, 340, 60) + + # 绘制红色渐变背景 + for i in range(bg_rect.height): + # 创建渐变红色 + alpha = int(100 * (1.0 - i / bg_rect.height)) + color = (intensity, 0, 0, alpha) + line_rect = self.pygame.Rect(bg_rect.x, bg_rect.y + i, bg_rect.width, 1) + self.pygame.draw.rect(self.main.surface, color, line_rect) + + # 绘制边框（闪烁） + border_color = (intensity, intensity // 2, 0) + self.pygame.draw.rect(self.main.surface, border_color, bg_rect, 4) + + # 显示警告文字 + warning_font = self.pygame.freetype.SysFont('Arial', 32) + warning_font.render_to(self.main.surface, (pos_x, pos_y), warning_text, (intensity, intensity, 0)) + + # 添加警告图标 + icon_x = pos_x + 280 + icon_y = pos_y + 20 + self.pygame.draw.circle(self.main.surface, (intensity, 0, 0), (icon_x, icon_y), 20) + + # 绘制感叹号 + exclamation_font = self.pygame.freetype.SysFont('Arial', 24) + exclamation_font.render_to(self.main.surface, (icon_x - 5, icon_y - 15), "!", (255, 255, 255)) + + # 添加副标题 + subtitle_text = "Reduce Speed and Steer Carefully" + subtitle_font = self.pygame.freetype.SysFont('Arial', 16) + subtitle_font.render_to(self.main.surface, (pos_x, pos_y + 40), subtitle_text, (255, 200, 0)) + + # 绘制警告脉冲效果 + pulse_radius = 15 + 5 * math.sin(self.frame_count * 0.2) + self.pygame.draw.circle(self.main.surface, (intensity, 0, 0, 100), (icon_x, icon_y), int(pulse_radius), 2) + + # 显示驾驶评分 + def display_driving_score(self, score, score_factors, score_history): + """在屏幕左侧中部显示驾驶评分""" + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 设置显示位置 + pos_x = 30 + pos_y = 180 + + # 根据评分设置颜色 + if score >= 85: + color = (0, 255, 0) # 优秀 - 绿色 + grade = "A" + elif score >= 70: + color = (255, 255, 0) # 良好 - 黄色 + grade = "B" + elif score >= 60: + color = (255, 165, 0) # 及格 - 橙色 + grade = "C" 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 + color = (255, 0, 0) # 差 - 红色 + grade = "D" + + # 绘制评分背景框 + bg_rect = self.pygame.Rect(pos_x - 10, pos_y - 10, 300, 200) + self.pygame.draw.rect(self.main.surface, (20, 20, 20, 200), bg_rect) + self.pygame.draw.rect(self.main.surface, (100, 100, 100), bg_rect, 2) + + # 显示标题 + title_text = "DRIVING PERFORMANCE" + self.steer_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) + + # 显示综合评分 + score_text = f"Score: {score:.1f}/100 ({grade})" + self.steer_font.render_to(self.main.surface, (pos_x, pos_y + 35), score_text, color) + + # 显示各项评分因子 + y_offset = 75 + factor_colors = { + 'speed_stability': (100, 200, 255), # 蓝色 + 'steering_smoothness': (255, 200, 100), # 橙色 + 'brake_usage': (255, 100, 100), # 红色 + 'path_following': (100, 255, 150), # 绿色 + 'safety': (200, 100, 255) # 紫色 } - 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) - + factor_labels = { + 'speed_stability': "Speed Stability", + 'steering_smoothness': "Steering Smooth", + 'brake_usage': "Brake Usage", + 'path_following': "Path Following", + 'safety': "Safety" + } -if __name__ == '__main__': - Main() \ No newline at end of file + for factor, label in factor_labels.items(): + factor_score = score_factors.get(factor, 0) + factor_color = factor_colors.get(factor, (200, 200, 200)) + + # 显示因子标签和分数 + factor_text = f"{label}: {factor_score:.1f}" + self.info_font.render_to(self.main.surface, (pos_x, pos_y + y_offset), factor_text, factor_color) + + # 绘制分数条 + bar_width = 150 + bar_height = 8 + bar_x = pos_x + 120 + bar_y = pos_y + y_offset + 5 + + # 绘制背景条 + 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) + + # 绘制填充条 + filled_width = int(bar_width * factor_score / 100) + filled_rect = self.pygame.Rect(bar_x, bar_y, filled_width, bar_height) + self.pygame.draw.rect(self.main.surface, factor_color, filled_rect) + + # 绘制边框 + self.pygame.draw.rect(self.main.surface, (150, 150, 150), bar_bg_rect, 1) + + y_offset += 20 + + # 绘制评分趋势图 + if len(score_history) >= 2: + chart_x = pos_x + chart_y = pos_y + 180 + chart_width = 280 + chart_height = 60 + + # 绘制图表背景 + chart_bg = self.pygame.Rect(chart_x, chart_y, chart_width, chart_height) + self.pygame.draw.rect(self.main.surface, (15, 15, 15), chart_bg) + self.pygame.draw.rect(self.main.surface, (80, 80, 80), chart_bg, 1) + + # 绘制趋势线 + points = [] + max_history = min(50, len(score_history)) # 最多显示最近50个点 + + for i in range(max_history): + idx = len(score_history) - max_history + i + if idx >= 0: + x = chart_x + int(i * chart_width / max_history) + # 分数范围0-100映射到图表高度 + y = chart_y + chart_height - int(score_history[idx] * chart_height / 100) + points.append((x, y)) + + if len(points) > 1: + # 绘制趋势线 + self.pygame.draw.lines(self.main.surface, color, False, points, 2) + + # 绘制当前点 + if points: + last_point = points[-1] + self.pygame.draw.circle(self.main.surface, color, last_point, 4) + self.pygame.draw.circle(self.main.surface, (255, 255, 255), last_point, 2) + + # 添加图表标签 + chart_label = "Score Trend" + self.info_font.render_to(self.main.surface, (chart_x, chart_y - 15), chart_label, (150, 150, 150)) + + # 显示航点导航信息 + def display_waypoint_navigation(self, current_index, waypoints, distance_to_waypoint, reached_count, progress): + """在屏幕中央下方显示航点导航信息""" + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 设置显示位置（屏幕中央下方） + pos_x = screen_width // 2 - 150 + pos_y = screen_height - 100 + + # 绘制背景框 + bg_rect = self.pygame.Rect(pos_x - 15, pos_y - 15, 330, 90) + self.pygame.draw.rect(self.main.surface, (20, 20, 20, 200), bg_rect) + self.pygame.draw.rect(self.main.surface, (80, 80, 200), bg_rect, 2) + + # 显示标题 + title_text = "WAYPOINT NAVIGATION" + title_font = self.pygame.freetype.SysFont('Arial', 20) + title_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (100, 200, 255)) + + if len(waypoints) == 0: + # 没有航点时显示提示 + no_waypoints_text = "No waypoints available" + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 30), no_waypoints_text, (150, 150, 150)) + return + + # 显示当前航点信息 + waypoint_info = f"Waypoint: {current_index + 1}/{len(waypoints)}" + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 30), waypoint_info, (255, 255, 255)) + + # 显示到航点的距离 + distance_text = f"Distance: {distance_to_waypoint:.1f}m" + distance_color = (255, 200, 100) if distance_to_waypoint > 10 else (100, 255, 100) + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 50), distance_text, distance_color) + + # 显示已到达航点数量 + reached_text = f"Reached: {reached_count}" + self.info_font.render_to(self.main.surface, (pos_x + 180, pos_y + 30), reached_text, (200, 200, 100)) + + # 绘制航点进度条 + progress_bar_width = 200 + progress_bar_height = 8 + progress_bar_x = pos_x + progress_bar_y = pos_y + 70 + + # 绘制进度条背景 + progress_bg_rect = self.pygame.Rect(progress_bar_x, progress_bar_y, progress_bar_width, progress_bar_height) + self.pygame.draw.rect(self.main.surface, (50, 50, 50), progress_bg_rect) + + # 绘制进度条填充 + filled_width = int(progress_bar_width * progress) + filled_rect = self.pygame.Rect(progress_bar_x, progress_bar_y, filled_width, progress_bar_height) + + # 根据进度设置颜色 + if progress < 0.33: + progress_color = (255, 100, 100) # 红色 - 起始 + elif progress < 0.66: + progress_color = (255, 200, 100) # 橙色 - 中间 + else: + progress_color = (100, 255, 100) # 绿色 - 接近完成 + + self.pygame.draw.rect(self.main.surface, progress_color, filled_rect) + self.pygame.draw.rect(self.main.surface, (200, 200, 200), progress_bg_rect, 1) + + # 显示进度百分比 + progress_text = f"{progress * 100:.0f}%" + progress_font = self.pygame.freetype.SysFont('Arial', 12) + progress_font.render_to(self.main.surface, (progress_bar_x + progress_bar_width + 5, progress_bar_y - 2), + progress_text, (200, 200, 200)) + + # 在屏幕上绘制航点指示器（小圆点） + self.draw_waypoint_indicators(waypoints, current_index) + + def draw_waypoint_indicators(self, waypoints, current_index): + """在屏幕上绘制航点指示器""" + if len(waypoints) == 0: + return + + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 航点指示器区域（屏幕顶部） + indicator_area_y = 80 + indicator_spacing = screen_width / (len(waypoints) + 1) + + for i, waypoint in enumerate(waypoints): + # 计算指示器位置 + indicator_x = int((i + 1) * indicator_spacing) + indicator_y = indicator_area_y + + # 根据航点状态设置颜色 + if i < current_index: + color = (100, 255, 100, 150) # 已通过 - 绿色半透明 + radius = 6 + elif i == current_index: + color = (255, 200, 0) # 当前目标 - 黄色 + radius = 10 + else: + color = (200, 200, 200, 100) # 未到达 - 灰色半透明 + radius = 8 + + # 绘制航点指示器 + self.pygame.draw.circle(self.main.surface, color, (indicator_x, indicator_y), radius) + + # 如果是当前航点，添加脉冲效果 + if i == current_index: + pulse_radius = radius + 3 + 2 * math.sin(self.frame_count * 0.1) + self.pygame.draw.circle(self.main.surface, (255, 200, 0, 100), (indicator_x, indicator_y), + int(pulse_radius), 2) + + # 添加航点编号 + waypoint_text = f"{i + 1}" + waypoint_font = self.pygame.freetype.SysFont('Arial', 12) + waypoint_font.render_to(self.main.surface, (indicator_x - 5, indicator_y - 20), waypoint_text, + (255, 255, 255)) + + # 在顶部显示航点导航标题 + if i == 0: + nav_title = "WAYPOINTS" + title_font = self.pygame.freetype.SysFont('Arial', 14) + title_font.render_to(self.main.surface, (screen_width // 2 - 40, indicator_y - 40), nav_title, + (150, 200, 255)) + + @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 diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index e7c6795370..02b89db5eb 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -52,6 +52,13 @@ def __init__(self): 'safety': 0.0 # 安全性 } + # 航点导航系统 + self.current_waypoint_index = 0 # 当前航点索引 + self.waypoint_positions = [] # 航点位置列表 + self.distance_to_waypoint = 0.0 # 到当前航点的距离 + self.waypoint_reached_count = 0 # 已到达航点计数 + self.waypoint_progress = 0.0 # 航点进度（0-1） + # start game loop self.game.game_loop(self.world, self.on_tick) @@ -70,6 +77,9 @@ def on_tick(self): wp = _wps[0] path.append(wp.transform.location) + # 更新航点信息 + self.update_waypoint_navigation(path) + # get forward speed velocity = self.ego.get_velocity() speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) @@ -187,6 +197,13 @@ def on_tick(self): 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) + self.drawer.display_waypoint_navigation( + self.current_waypoint_index, + self.waypoint_positions, + self.distance_to_waypoint, + self.waypoint_reached_count, + self.waypoint_progress + ) def check_collision_warning(self, path, speed_kmh, steer_angle): """检测可能的碰撞风险""" @@ -289,6 +306,53 @@ def calculate_driving_score(self, current_speed, steer_angle, brake_force, path) print(f"安全性: {safety:.1f}") print("=" * 40) + def update_waypoint_navigation(self, path): + """更新航点导航信息""" + if len(path) == 0: + return + + # 保存航点位置 + self.waypoint_positions = path + + # 计算车辆当前位置 + vehicle_location = self.ego.get_location() + + # 如果还没有设置当前航点，从第一个开始 + if self.current_waypoint_index >= len(self.waypoint_positions): + self.current_waypoint_index = 0 + + # 计算到当前航点的距离 + current_waypoint = self.waypoint_positions[self.current_waypoint_index] + dx = current_waypoint.x - vehicle_location.x + dy = current_waypoint.y - vehicle_location.y + self.distance_to_waypoint = math.sqrt(dx * dx + dy * dy) + + # 检查是否到达当前航点（距离小于5米） + waypoint_threshold = 5.0 # 到达阈值（米） + if self.distance_to_waypoint < waypoint_threshold: + # 到达当前航点，切换到下一个 + self.current_waypoint_index += 1 + self.waypoint_reached_count += 1 + + # 如果到达所有航点，重新开始 + if self.current_waypoint_index >= len(self.waypoint_positions): + self.current_waypoint_index = 0 + + # 重新计算到新航点的距离 + if self.current_waypoint_index < len(self.waypoint_positions): + new_waypoint = self.waypoint_positions[self.current_waypoint_index] + dx = new_waypoint.x - vehicle_location.x + dy = new_waypoint.y - vehicle_location.y + self.distance_to_waypoint = math.sqrt(dx * dx + dy * dy) + + # 输出到达信息 + print( + f"到达航点 #{self.waypoint_reached_count}，切换到航点 {self.current_waypoint_index + 1}/{len(self.waypoint_positions)}") + + # 计算航点进度（0-1） + if len(self.waypoint_positions) > 0: + self.waypoint_progress = self.current_waypoint_index / len(self.waypoint_positions) + if __name__ == '__main__': Main() \ No newline at end of file From 17d53a4302f5763f6fa1724011fac707eba9d6b2 Mon Sep 17 00:00:00 2001 From: 183899 Date: Mon, 22 Dec 2025 17:02:13 +0800 Subject: [PATCH 11/14] =?UTF-8?q?=E6=A0=B9=E6=8D=AE=E7=8E=B0=E6=9C=89?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=BF=AE=E6=94=B9REDME=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/RADME.md | 110 ++++++++++++++++++++++++++++++++--- 1 file changed, 103 insertions(+), 7 deletions(-) diff --git a/src/unmannedcar_MPC/RADME.md b/src/unmannedcar_MPC/RADME.md index 3f7d1cc7a7..ff4da0f760 100644 --- a/src/unmannedcar_MPC/RADME.md +++ b/src/unmannedcar_MPC/RADME.md @@ -1,14 +1,110 @@ -这是一个基于CARLA模拟器的主动式车道保持辅助系统项目，它就像一个虚拟的智能驾驶员，能够通过摄像头观察道路情况，自动控制方向盘使车辆稳定行驶在车道中心。 +readme.py - 项目说明文档 -整个系统的运作始于一个摄像头，它安装在虚拟车辆的前方，持续捕捉前方的道路图像。这些彩色图像被送入一个视觉处理程序，首先将图像从常见的RGB色彩空间转换到HLS空间，这是因为车道线在这种色彩模式下更容易被识别。程序会过滤出白色和黄色的像素，这些能就是车道线。接着，通过一种叫做透视变换的技术，把图像转换成鸟瞰图，仿佛我们从天空俯视道路一样，这样能更准确地判断车道线的弯曲程度和车辆的位置。然后，系统采用滑动窗口的方法，从图像底部开始，像爬楼梯一样一层层向上寻找属于车道线的像素点，再用一条光滑的曲线去拟合这些点，从而得到两条清晰的车道边界。最后，计算出车辆中心点与这两条边界中心线的横向距离，这个距离就是车辆偏离车道中心的误差。 +运行此文件查看完整项目说明 -环境安装：python -m venv venv +README\_TEXT = """ -. venv/bin/activate +一、项目概述 -pip install --upgrade pip +这是一个基于Carla仿真平台和MPC控制器的自动驾驶系统，包含完整的驾驶评估 -pip install -r requirements.txt +和导航功能。 -结果：生成一辆小车，按照预定轨迹提前改变路径，实现控制预测与转弯。 +二、主要功能 + +1\. MPC路径跟踪控制 + +2\. 智能速度控制（自动加速/刹车） + +3\. 驾驶评分系统（5个维度实时评分） + +4\. 碰撞预警系统 + +5\. 航点导航系统 + +6\. 完整的可视化界面 + +三、新增功能说明 + +1\. 航点导航系统 + +  - 实时跟踪车辆航点进度 + +  - 显示当前航点序号和距离 + +  - 航点进度条可视化 + +  - 屏幕顶部航点状态指示器 + +2\. 驾驶评分系统 + +  - 综合评分（0-100分，A/B/C/D等级） + +  - 五个评分维度：速度稳定性、转向平滑度、刹车使用、路径跟踪、安全性 + +  - 评分趋势图表 + +3\. 其他显示功能 + +  - 速度显示（带颜色编码） + +  - 刹车状态显示 + +  - 转向角度显示 + +  - 油门/刹车输入显示 + +  - 碰撞警告显示 + +  - 帧率信息显示 + +四、文件结构 + +├── config.py # 配置文件 + +├── main.py # 主程序（包含所有控制逻辑） + +├── drawer.py # 绘图显示模块 + +├── mpc.py # MPC控制器 + +├── readme.py # 项目说明文档 + +└── requirements.txt # 依赖包列表 + +五、运行方法 + +1\. 启动Carla服务器：./CarlaUE4.sh + +2\. 运行项目：python main.py + +3\. 查看说明：python readme.py + +六、配置说明 + +在config.py中可以修改： + +\- 服务器地址和地图 + +\- 屏幕分辨率和帧率 + +\- 相机视野角度 + +七、依赖包 + +\- carla + +\- pygame + +\- numpy + +\- scipy + +""" + + + +if \_\_name\_\_ == "\_\_main\_\_": + +  print(README\_TEXT) From 485447c0045101deb2a9cdcd8dbde9dd805ebac1 Mon Sep 17 00:00:00 2001 From: 183899 Date: Wed, 24 Dec 2025 20:05:25 +0800 Subject: [PATCH 12/14] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=87=BD=E6=95=B0?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=EF=BC=8C=E5=A2=9E=E6=B7=BB=E8=AF=84=E4=BB=B7?= =?UTF-8?q?=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/drawer.py | 418 ++++++++++++++++++++++++++++++++++ src/unmannedcar_MPC/main.py | 151 +++++++++++- 2 files changed, 568 insertions(+), 1 deletion(-) diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py index e5c118a84e..1b8a890195 100644 --- a/src/unmannedcar_MPC/drawer.py +++ b/src/unmannedcar_MPC/drawer.py @@ -1,8 +1,11 @@ +#!/usr/bin/env python3 + import carla import config as Config import numpy as np import math import time +import random class PyGameDrawer(): @@ -26,6 +29,11 @@ def __init__(self, main): # 通用信息字体 self.info_font = self.pygame.freetype.SysFont('Arial', 16) + # 驾驶辅助线相关字体 + self.assist_font = self.pygame.freetype.SysFont('Arial', 18) + self.assist_font_small = self.pygame.freetype.SysFont('Arial', 14) + self.radar_font = self.pygame.freetype.SysFont('Arial', 12) + # 初始化时间 self.start_time = time.time() self.frame_count = 0 # 帧计数器 @@ -797,6 +805,416 @@ def draw_waypoint_indicators(self, waypoints, current_index): title_font.render_to(self.main.surface, (screen_width // 2 - 40, indicator_y - 40), nav_title, (150, 200, 255)) + # 显示驾驶辅助线 + def display_driving_assist_lines(self, vehicle_location, vehicle_transform, steer_angle, path=None): + """在屏幕上显示驾驶辅助线和预期路径""" + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 设置显示位置（屏幕中央） + center_x = screen_width // 2 + center_y = screen_height // 2 + + # 1. 绘制车辆中心参考线 + # 绘制垂直中心线 + self.pygame.draw.line( + self.main.surface, + (0, 255, 0, 100), # 半透明绿色 + (center_x, center_y - 50), + (center_x, center_y + 150), + 1 + ) + + # 绘制水平中心线 + self.pygame.draw.line( + self.main.surface, + (0, 255, 0, 100), # 半透明绿色 + (center_x - 100, center_y), + (center_x + 100, center_y), + 1 + ) + + # 2. 绘制转向辅助线 + # 根据转向角度计算辅助线的方向和长度 + turn_radius = 200 # 基础转弯半径 + if abs(steer_angle) > 0.01: # 有转向时 + # 计算转向曲率 + curvature = steer_angle * 2.0 + arc_radius = int(turn_radius / (abs(curvature) + 0.1)) + + # 计算弧线的起点、终点和中心 + if steer_angle > 0: # 右转 + center_offset = arc_radius + arc_color = (255, 100, 0, 150) # 橙色 + else: # 左转 + center_offset = -arc_radius + arc_color = (255, 200, 0, 150) # 黄色 + + # 绘制转向弧线 + arc_center_x = center_x + center_offset + arc_center_y = center_y + 100 + + # 计算弧线角度范围 + start_angle = 180 if steer_angle > 0 else 0 + end_angle = 0 if steer_angle > 0 else 180 + + # 绘制弧线 + self.pygame.draw.arc( + self.main.surface, + arc_color, + (arc_center_x - arc_radius, arc_center_y - arc_radius, + arc_radius * 2, arc_radius * 2), + math.radians(start_angle), + math.radians(end_angle), + 3 + ) + + # 绘制转向方向指示箭头 + arrow_length = 30 + if steer_angle > 0: # 右转箭头 + arrow_points = [ + (center_x + 150, center_y + 50), + (center_x + 150 - arrow_length, center_y + 50 - arrow_length // 2), + (center_x + 150 - arrow_length, center_y + 50 + arrow_length // 2) + ] + else: # 左转箭头 + arrow_points = [ + (center_x - 150, center_y + 50), + (center_x - 150 + arrow_length, center_y + 50 - arrow_length // 2), + (center_x - 150 + arrow_length, center_y + 50 + arrow_length // 2) + ] + + self.pygame.draw.polygon( + self.main.surface, + arc_color, + arrow_points + ) + + # 显示转向半径 + radius_text = f"R: {arc_radius / 10:.1f}m" + radius_font = self.pygame.freetype.SysFont('Arial', 14) + radius_font.render_to( + self.main.surface, + (arc_center_x - 30, arc_center_y - arc_radius - 20), + radius_text, + arc_color + ) + + # 3. 绘制安全距离参考线 + # 基于速度的安全距离（假设1秒反应距离） + speed_m_s = 0 # 如果没有速度信息，默认为0 + if hasattr(self.main, 'ego'): + velocity = self.main.ego.get_velocity() + speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + + safe_distance = speed_m_s * 1.5 # 1.5秒的安全距离 + + # 绘制安全距离线（红色） + safe_line_y = center_y + 100 - int(safe_distance * 5) # 缩放因子 + if safe_line_y > center_y - 100: # 确保在屏幕内 + self.pygame.draw.line( + self.main.surface, + (255, 0, 0, 100), # 半透明红色 + (center_x - 80, safe_line_y), + (center_x + 80, safe_line_y), + 2 + ) + + # 标注安全距离 + safe_text = f"Safe: {safe_distance:.1f}m" + safe_font = self.pygame.freetype.SysFont('Arial', 12) + safe_font.render_to( + self.main.surface, + (center_x + 90, safe_line_y - 10), + safe_text, + (255, 0, 0) + ) + + # 4. 绘制车道保持辅助线 + # 绘制车道边界线（蓝色虚线） + lane_width = 80 # 车道宽度 + left_lane_x = center_x - lane_width + right_lane_x = center_x + lane_width + + # 绘制左车道线（蓝色虚线） + for i in range(0, 200, 10): + if i % 20 < 10: # 创建虚线效果 + self.pygame.draw.line( + self.main.surface, + (100, 100, 255, 150), # 半透明蓝色 + (left_lane_x, center_y + i), + (left_lane_x, center_y + i + 5), + 2 + ) + + # 绘制右车道线（蓝色虚线） + for i in range(0, 200, 10): + if i % 20 < 10: # 创建虚线效果 + self.pygame.draw.line( + self.main.surface, + (100, 100, 255, 150), # 半透明蓝色 + (right_lane_x, center_y + i), + (right_lane_x, center_y + i + 5), + 2 + ) + + # 5. 如果提供了路径，绘制预期路径 + if path and len(path) > 1: + # 转换路径点到屏幕坐标 + path_points = [] + for i, location in enumerate(path): + if i >= 10: # 只显示前10个路径点 + break + + # 将世界坐标转换为屏幕坐标 + cam_loc = self.__w_locs_2_camera_locs([location])[0] + + # 只添加在屏幕内的点 + if (0 <= cam_loc[0] <= screen_width and + 0 <= cam_loc[1] <= screen_height): + path_points.append(cam_loc) + + # 绘制路径线（绿色虚线） + if len(path_points) > 1: + for i in range(len(path_points) - 1): + # 创建渐变颜色：近处明亮，远处暗淡 + alpha = int(255 * (1.0 - i / len(path_points))) + color = (0, 255, 0, alpha) + + # 绘制虚线 + if i % 2 == 0: + self.pygame.draw.line( + self.main.surface, + color, + path_points[i], + path_points[i + 1], + 2 + ) + + # 在最后一个路径点上绘制标记 + if path_points: + last_point = path_points[-1] + self.pygame.draw.circle( + self.main.surface, + (255, 255, 0), # 黄色 + last_point, + 5 + ) + + # 6. 绘制车辆当前位置指示器 + # 在屏幕中心绘制一个车辆图标 + vehicle_color = (0, 200, 255) # 青色 + + # 绘制车辆主体（矩形） + vehicle_rect = self.pygame.Rect(center_x - 15, center_y - 25, 30, 50) + self.pygame.draw.rect( + self.main.surface, + vehicle_color, + vehicle_rect, + 2 + ) + + # 绘制车辆前进方向箭头 + arrow_length = 40 + self.pygame.draw.line( + self.main.surface, + (255, 255, 0), # 黄色箭头 + (center_x, center_y), + (center_x, center_y - arrow_length), + 3 + ) + + # 绘制箭头头部 + arrow_head = [ + (center_x, center_y - arrow_length), + (center_x - 5, center_y - arrow_length + 10), + (center_x + 5, center_y - arrow_length + 10) + ] + self.pygame.draw.polygon( + self.main.surface, + (255, 255, 0), + arrow_head + ) + + # 7. 显示辅助系统状态 + assist_font = self.pygame.freetype.SysFont('Arial', 16) + assist_text = "DRIVING ASSIST" + assist_font.render_to( + self.main.surface, + (center_x - 50, center_y - 80), + assist_text, + (200, 200, 255) + ) + + # 根据转向角度显示转向辅助状态 + if abs(steer_angle) > 0.1: + turn_status = "TURNING" + turn_color = (255, 200, 0) + else: + turn_status = "STRAIGHT" + turn_color = (0, 255, 0) + + status_font = self.pygame.freetype.SysFont('Arial', 14) + status_font.render_to( + self.main.surface, + (center_x - 40, center_y - 60), + turn_status, + turn_color + ) + + # 显示简单雷达图（检测周围环境） + def display_simple_radar(self, vehicle_location, obstacles=None): + """在屏幕右下角显示简单的雷达图，显示周围环境""" + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 雷达图位置和大小 + radar_x = screen_width - 180 + radar_y = screen_height - 180 + radar_radius = 70 + + # 绘制雷达图背景（圆形） + self.pygame.draw.circle( + self.main.surface, + (20, 20, 40), # 深蓝色背景 + (radar_x, radar_y), + radar_radius + ) + + # 绘制雷达图网格 + # 同心圆 + for i in range(1, 4): + radius = int(radar_radius * i / 4) + self.pygame.draw.circle( + self.main.surface, + (50, 50, 80), # 网格颜色 + (radar_x, radar_y), + radius, + 1 + ) + + # 十字线 + self.pygame.draw.line( + self.main.surface, + (50, 50, 80), + (radar_x - radar_radius, radar_y), + (radar_x + radar_radius, radar_y), + 1 + ) + self.pygame.draw.line( + self.main.surface, + (50, 50, 80), + (radar_x, radar_y - radar_radius), + (radar_x, radar_y + radar_radius), + 1 + ) + + # 绘制方向指示 + # 前方（上） + self.pygame.draw.line( + self.main.surface, + (100, 100, 200), + (radar_x, radar_y - radar_radius + 5), + (radar_x, radar_y - radar_radius + 15), + 2 + ) + + # 绘制车辆位置（中心点） + self.pygame.draw.circle( + self.main.surface, + (0, 255, 0), # 绿色表示车辆 + (radar_x, radar_y), + 4 + ) + + # 如果提供了障碍物信息，绘制障碍物 + if obstacles: + for obstacle in obstacles: + # 获取障碍物信息 + if isinstance(obstacle, dict): + # 从main.py传递的障碍物字典 + distance = obstacle.get('distance', 0) + angle = obstacle.get('angle', 0) + + # 限制距离在雷达范围内 + normalized_distance = min(distance / 50.0, 1.0) # 假设最大检测距离50米 + + # 计算障碍物在雷达图上的位置 + obstacle_radius = int(radar_radius * normalized_distance) + obstacle_angle = math.radians(angle) + + obstacle_x = radar_x + int(obstacle_radius * math.sin(obstacle_angle)) + obstacle_y = radar_y - int(obstacle_radius * math.cos(obstacle_angle)) + + # 根据障碍物类型设置颜色 + obstacle_type = obstacle.get('type', 'unknown') + if obstacle_type == 'vehicle': + color = (255, 100, 100) # 红色表示车辆 + elif obstacle_type == 'static': + color = (200, 200, 100) # 黄色表示静态障碍物 + else: + color = (150, 150, 150) # 灰色表示未知障碍物 + + # 绘制障碍物点 + self.pygame.draw.circle( + self.main.surface, + color, + (obstacle_x, obstacle_y), + 4 + ) + + # 如果障碍物很近，添加警告效果 + if distance < 10.0: + pulse_radius = 4 + 2 * math.sin(self.frame_count * 0.2) + self.pygame.draw.circle( + self.main.surface, + (255, 0, 0, 100), + (obstacle_x, obstacle_y), + int(pulse_radius), + 1 + ) + + # 绘制雷达扫描线（旋转效果） + scan_angle = (self.frame_count * 2) % 360 # 根据帧数旋转 + scan_end_x = radar_x + int(radar_radius * math.sin(math.radians(scan_angle))) + scan_end_y = radar_y - int(radar_radius * math.cos(math.radians(scan_angle))) # 注意：屏幕y轴向下为正 + + # 绘制扫描线 + self.pygame.draw.line( + self.main.surface, + (0, 255, 0, 100), # 半透明绿色 + (radar_x, radar_y), + (scan_end_x, scan_end_y), + 1 + ) + + # 绘制雷达图标题 + radar_font = self.pygame.freetype.SysFont('Arial', 14) + radar_font.render_to( + self.main.surface, + (radar_x - 40, radar_y - radar_radius - 20), + "RADAR", + (100, 200, 255) + ) + + # 添加图例 + legend_font = self.pygame.freetype.SysFont('Arial', 10) + legend_font.render_to( + self.main.surface, + (radar_x - radar_radius, radar_y + radar_radius + 10), + "● Vehicle ● Static", + (150, 150, 150) + ) + + # 显示检测范围 + range_text = "Range: 50m" + legend_font.render_to( + self.main.surface, + (radar_x - radar_radius, radar_y + radar_radius + 25), + range_text, + (100, 100, 200) + ) + @staticmethod def get_location_bbox(location, camera): bb_cords = np.array([[0, 0, 0, 1]]) diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index 02b89db5eb..d0c3a0c9b2 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -59,6 +59,15 @@ def __init__(self): self.waypoint_reached_count = 0 # 已到达航点计数 self.waypoint_progress = 0.0 # 航点进度（0-1） + # 障碍物检测系统 + self.obstacles = [] # 障碍物列表 + self.obstacle_detection_range = 50.0 # 障碍物检测范围（米） + + # 驾驶辅助系统状态 + self.lane_assist_active = True # 车道保持辅助状态 + self.adaptive_cruise_active = True # 自适应巡航状态 + self.collision_avoidance_active = True # 碰撞避免状态 + # start game loop self.game.game_loop(self.world, self.on_tick) @@ -80,6 +89,9 @@ def on_tick(self): # 更新航点信息 self.update_waypoint_navigation(path) + # 检测障碍物 + self.detect_obstacles() + # get forward speed velocity = self.ego.get_velocity() speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) @@ -128,6 +140,15 @@ def on_tick(self): else: speed_error = current_speed_kmh - self.target_speed_kmh + # 根据前方障碍物调整速度 + if self.obstacles: + # 如果有障碍物，进一步降低目标速度 + min_obstacle_distance = min(self.obstacles, key=lambda x: x['distance'])['distance'] + if min_obstacle_distance < 20.0: # 20米内有障碍物 + safety_factor = min_obstacle_distance / 20.0 # 距离越近，因子越小 + self.target_speed_kmh = min(self.target_speed_kmh, 30.0 * safety_factor) + 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) # 快速增加刹车力度 @@ -205,14 +226,32 @@ def on_tick(self): self.waypoint_progress ) + # 显示驾驶辅助线和雷达图 + self.drawer.display_driving_assist_lines( + self.ego.get_location(), + self.ego.get_transform(), + self.steer_angle, + path # 传入路径用于绘制预期路径 + ) + + # 显示雷达图（传入检测到的障碍物） + self.drawer.display_simple_radar(self.ego.get_location(), self.obstacles) + def check_collision_warning(self, path, speed_kmh, steer_angle): """检测可能的碰撞风险""" # 基于转向角度和速度的简单碰撞检测 speed_factor = speed_kmh / 100.0 # 速度越快，风险越高 steer_factor = abs(steer_angle) # 转向角度越大，风险越高 + # 考虑障碍物距离 + obstacle_factor = 0.0 + if self.obstacles: + min_obstacle_distance = min(self.obstacles, key=lambda x: x['distance'])['distance'] + if min_obstacle_distance < 10.0: + obstacle_factor = 1.0 - (min_obstacle_distance / 10.0) + # 计算碰撞风险 - collision_risk = speed_factor * (1.0 + steer_factor * 3) + collision_risk = speed_factor * (1.0 + steer_factor * 3) + obstacle_factor * 0.5 # 检查是否超过阈值 warning_threshold = 0.5 @@ -228,6 +267,8 @@ def check_collision_warning(self, path, speed_kmh, steer_angle): if self.collision_warning != was_warning: if self.collision_warning: print(f"碰撞警告激活！速度: {speed_kmh:.1f} km/h, 转向: {steer_angle:.3f}, 风险: {collision_risk:.2f}") + if self.obstacles: + print(f" 最近障碍物距离: {min(self.obstacles, key=lambda x: x['distance'])['distance']:.1f}米") else: print("碰撞警告解除") @@ -265,6 +306,8 @@ def calculate_driving_score(self, current_speed, steer_angle, brake_force, path) safety_penalty += 30 # 碰撞警告扣分 if brake_force > 0.5: safety_penalty += 20 # 紧急刹车扣分 + if self.obstacles and min(self.obstacles, key=lambda x: x['distance'])['distance'] < 5.0: + safety_penalty += 30 # 距离障碍物太近扣分 safety = max(0, 100 - safety_penalty) # 保存各项评分因子 @@ -353,6 +396,112 @@ def update_waypoint_navigation(self, path): if len(self.waypoint_positions) > 0: self.waypoint_progress = self.current_waypoint_index / len(self.waypoint_positions) + def detect_obstacles(self): + """检测车辆周围的障碍物""" + # 清空障碍物列表 + self.obstacles = [] + + # 获取车辆位置和朝向 + vehicle_location = self.ego.get_location() + vehicle_transform = self.ego.get_transform() + vehicle_rotation = vehicle_transform.rotation + + # 获取车辆前方的航点作为参考方向 + wp = self.map.get_waypoint(vehicle_location) + + # 检测周围的车辆 + vehicle_list = self.world.get_actors().filter('vehicle.*') + + for vehicle in vehicle_list: + # 排除自车 + if vehicle.id == self.ego.id: + continue + + # 计算车辆距离 + other_location = vehicle.get_location() + distance = vehicle_location.distance(other_location) + + # 只检测一定范围内的车辆 + if distance < self.obstacle_detection_range: + # 计算相对方向 + dx = other_location.x - vehicle_location.x + dy = other_location.y - vehicle_location.y + + # 计算相对于车辆前进方向的角度 + # 这里简化处理，使用航点方向作为参考 + forward_vector = vehicle_transform.get_forward_vector() + relative_vector = carla.Vector3D(dx, dy, 0) + + # 计算点积和叉积 + dot_product = forward_vector.x * relative_vector.x + forward_vector.y * relative_vector.y + cross_product = forward_vector.x * relative_vector.y - forward_vector.y * relative_vector.x + + # 计算角度（弧度） + angle = math.atan2(cross_product, dot_product) + + # 转换为度 + angle_deg = math.degrees(angle) + + # 只考虑前方±60度范围内的障碍物 + if abs(angle_deg) < 60: + # 计算相对速度（简化） + other_velocity = vehicle.get_velocity() + relative_speed = math.sqrt( + (other_velocity.x - self.ego.get_velocity().x) ** 2 + + (other_velocity.y - self.ego.get_velocity().y) ** 2 + ) + + # 添加到障碍物列表 + self.obstacles.append({ + 'location': other_location, + 'distance': distance, + 'angle': angle_deg, + 'relative_speed': relative_speed, + 'type': 'vehicle' + }) + + # 检测静态障碍物（简化：使用地图中的建筑） + # 这里简化处理，实际上应该使用传感器数据 + if self.frame_count % 30 == 0: # 每30帧检测一次静态障碍物 + # 随机添加一些模拟的静态障碍物用于演示 + import random + for i in range(random.randint(0, 3)): + # 在车辆前方随机位置添加模拟障碍物 + angle = random.uniform(-45, 45) + distance = random.uniform(10, 40) + + # 计算障碍物位置 + angle_rad = math.radians(angle) + obstacle_x = vehicle_location.x + distance * math.cos(angle_rad + math.radians(vehicle_rotation.yaw)) + obstacle_y = vehicle_location.y + distance * math.sin(angle_rad + math.radians(vehicle_rotation.yaw)) + obstacle_z = vehicle_location.z + + obstacle_location = carla.Location(x=obstacle_x, y=obstacle_y, z=obstacle_z) + + # 添加到障碍物列表 + self.obstacles.append({ + 'location': obstacle_location, + 'distance': distance, + 'angle': angle, + 'relative_speed': 0.0, + 'type': 'static' + }) + + # 按距离排序 + self.obstacles.sort(key=lambda x: x['distance']) + + # 保留最近的5个障碍物 + if len(self.obstacles) > 5: + self.obstacles = self.obstacles[:5] + + # 每100帧输出一次障碍物信息 + if self.frame_count % 100 == 0 and self.obstacles: + print(f"\n=== 障碍物检测报告 (帧 {self.frame_count}) ===") + for i, obstacle in enumerate(self.obstacles): + print(f"障碍物 {i + 1}: 距离={obstacle['distance']:.1f}米, 角度={obstacle['angle']:.1f}°, " + f"类型={obstacle['type']}, 相对速度={obstacle['relative_speed']:.1f} m/s") + print("=" * 40) + if __name__ == '__main__': Main() \ No newline at end of file From e8f56377fdd7f24a843fca33666f2eb3c1c26b92 Mon Sep 17 00:00:00 2001 From: 183899 Date: Thu, 25 Dec 2025 21:05:23 +0800 Subject: [PATCH 13/14] =?UTF-8?q?=E6=A0=B9=E6=8D=AE=E7=8E=B0=E6=9C=89?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E6=9B=B4=E6=96=B0REDME=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/RADME.md | 382 ++++++++++++++++++++++++++++++----- 1 file changed, 328 insertions(+), 54 deletions(-) diff --git a/src/unmannedcar_MPC/RADME.md b/src/unmannedcar_MPC/RADME.md index ff4da0f760..d3ebd28e36 100644 --- a/src/unmannedcar_MPC/RADME.md +++ b/src/unmannedcar_MPC/RADME.md @@ -1,110 +1,384 @@ -readme.py - 项目说明文档 +\#!/usr/bin/env python3 -运行此文件查看完整项目说明 +""" -README\_TEXT = """ +PyGameDrawer - 绘图显示模块完整说明文档 -一、项目概述 +运行此文件查看drawer.py中所有函数说明 -这是一个基于Carla仿真平台和MPC控制器的自动驾驶系统，包含完整的驾驶评估 +""" -和导航功能。 +一、模块概述 -二、主要功能 +PyGameDrawer是基于Pygame的图形绘制模块，用于在Carla仿真环境中显示各种驾驶信息。 -1\. MPC路径跟踪控制 +模块提供了丰富的可视化功能，包括速度显示、转向显示、驾驶评分、辅助线等。 -2\. 智能速度控制（自动加速/刹车） -3\. 驾驶评分系统（5个维度实时评分） -4\. 碰撞预警系统 +二、坐标转换函数 -5\. 航点导航系统 +1\. \_\_w\_locs\_2\_camera\_locs(w\_locs) -6\. 完整的可视化界面 +  - 功能：将世界坐标转换为相机屏幕坐标 -三、新增功能说明 +  - 参数：w\_locs - 世界坐标列表 -1\. 航点导航系统 +  - 返回：屏幕坐标列表 -  - 实时跟踪车辆航点进度 -  - 显示当前航点序号和距离 -  - 航点进度条可视化 +2\. draw\_camera\_text(location, color, text) -  - 屏幕顶部航点状态指示器 +  - 功能：在相机视角下绘制文本 -2\. 驾驶评分系统 +  - 参数：location - 世界坐标，color - 颜色，text - 文本内容 -  - 综合评分（0-100分，A/B/C/D等级） -  - 五个评分维度：速度稳定性、转向平滑度、刹车使用、路径跟踪、安全性 -  - 评分趋势图表 +3\. draw\_camera\_circles(w\_locs, color, radius) -3\. 其他显示功能 +  - 功能：在相机视角下绘制圆形 -  - 速度显示（带颜色编码） +  - 参数：w\_locs - 世界坐标列表，color - 颜色，radius - 半径 -  - 刹车状态显示 -  - 转向角度显示 -  - 油门/刹车输入显示 +4\. draw\_camera\_polygon(w\_locs, color) -  - 碰撞警告显示 +  - 功能：在相机视角下绘制多边形 -  - 帧率信息显示 +  - 参数：w\_locs - 世界坐标列表，color - 颜色 -四、文件结构 -├── config.py # 配置文件 -├── main.py # 主程序（包含所有控制逻辑） +5\. draw\_camera\_lines(color, w\_locs, width=1) -├── drawer.py # 绘图显示模块 +  - 功能：在相机视角下绘制线条 -├── mpc.py # MPC控制器 +  - 参数：color - 颜色，w\_locs - 世界坐标列表，width - 线宽 -├── readme.py # 项目说明文档 -└── requirements.txt # 依赖包列表 -五、运行方法 +6\. \_\_draw\_camera\_line\_safe(color, pt1, pt2, width=1) -1\. 启动Carla服务器：./CarlaUE4.sh +  - 功能：安全绘制线条（检查边界） -2\. 运行项目：python main.py +  - 参数：color - 颜色，pt1 - 起点，pt2 - 终点，width - 线宽 -3\. 查看说明：python readme.py -六、配置说明 -在config.py中可以修改： +7\. draw\_point(location, color, radius=3) -\- 服务器地址和地图 +  - 功能：在相机视角下绘制点 -\- 屏幕分辨率和帧率 +  - 参数：location - 世界坐标，color - 颜色，radius - 半径 -\- 相机视野角度 -七、依赖包 -\- carla +三、信息显示函数 -\- pygame +1\. display\_speed(speed\_kmh) -\- numpy +  - 功能：显示当前速度（右上角） -\- scipy +  - 参数：speed\_kmh - 速度值（km/h） + +  - 特性：颜色编码（绿/黄/橙/红），速度条可视化 + + + +2\. display\_brake\_status(is\_braking, brake\_history, target\_speed, frame\_count) + +  - 功能：显示刹车状态（左上角） + +  - 参数： + +  - is\_braking: 当前是否刹车 + +  - brake\_history: 刹车历史记录 + +  - target\_speed: 目标速度 + +  - frame\_count: 帧计数 + +  - 特性：闪烁效果，测试模式，颜色编码 + + + +3\. display\_speed\_history(speed\_history, target\_speed) + +  - 功能：显示速度历史图表（左下角） + +  - 参数： + +  - speed\_history: 速度历史记录 + +  - target\_speed: 目标速度 + +  - 特性：趋势线，目标线，当前速度标注 + + + +4\. display\_steering(steer\_angle) + +  - 功能：显示转向角度（右下角） + +  - 参数：steer\_angle - 转向角度值（-1到1） + +  - 特性：角度可视化，颜色编码，方向箭头 + + + +5\. display\_throttle\_info(throttle\_value, brake\_value) + +  - 功能：显示油门和刹车信息（右侧中部） + +  - 参数： + +  - throttle\_value: 油门值（0-1） + +  - brake\_value: 刹车值（0-1） + +  - 特性：条形图，实时更新 + + + +6\. display\_control\_mode(control\_mode) + +  - 功能：显示控制模式（顶部中央） + +  - 参数：control\_mode - 控制模式（AUTO/MANUAL） + +  - 特性：不同颜色标识 + + + +7\. display\_frame\_info(frame\_count, dt) + +  - 功能：显示帧信息（左下角） + +  - 参数： + +  - frame\_count: 帧计数 + +  - dt: 时间间隔 + +  - 特性：FPS计算，运行时间 + + + +8\. display\_collision\_warning(collision\_warning, collision\_history) + +  - 功能：显示碰撞警告（中央上方） + +  - 参数： + +  - collision\_warning: 当前碰撞警告状态 + +  - collision\_history: 碰撞警告历史 + +  - 特性：闪烁效果，脉冲动画，渐变背景 + + + +9\. display\_driving\_score(score, score\_factors, score\_history) + +  - 功能：显示驾驶评分（左侧中部） + +  - 参数： + +  - score: 综合评分 + +  - score\_factors: 各项评分因子 + +  - score\_history: 评分历史记录 + +  - 特性：等级评定（A/B/C/D），趋势图，五项评分指标 + + + +10\. display\_waypoint\_navigation(current\_index, waypoints, distance\_to\_waypoint, reached\_count, progress) + +  - 功能：显示航点导航信息（中央下方） + +  - 参数： + +  - current\_index: 当前航点索引 + +  - waypoints: 航点位置列表 + +  - distance\_to\_waypoint: 到当前航点的距离 + +  - reached\_count: 已到达航点计数 + +  - progress: 航点进度（0-1） + +  - 特性：进度条，航点指示器，脉冲效果 + + + +11\. draw\_waypoint\_indicators(waypoints, current\_index) + +  - 功能：绘制航点指示器（屏幕顶部） + +  - 参数： + +  - waypoints: 航点位置列表 + +  - current\_index: 当前航点索引 + +  - 特性：不同状态不同颜色，当前航点脉冲效果 -""" +四、驾驶辅助系统函数 -if \_\_name\_\_ == "\_\_main\_\_": +1\. display\_driving\_assist\_lines(vehicle\_location, vehicle\_transform, steer\_angle, path=None) -  print(README\_TEXT) +  - 功能：显示驾驶辅助线和预期路径（屏幕中央） + +  - 参数： + +  - vehicle\_location: 车辆位置 + +  - vehicle\_transform: 车辆变换 + +  - steer\_angle: 转向角度 + +  - path: 预期路径（可选） + +  - 特性： + +  - 中心参考线 + +  - 转向辅助线（弧线） + +  - 安全距离线 + +  - 车道保持辅助线 + +  - 预期路径显示 + +  - 车辆图标和前进方向 + + + +2\. display\_simple\_radar(vehicle\_location, obstacles=None) + +  - 功能：显示简单雷达图（检测周围环境）（右下角） + +  - 参数： + +  - vehicle\_location: 车辆位置 + +  - obstacles: 障碍物列表（可选） + +  - 特性： + +  - 雷达网格 + +  - 障碍物显示（不同颜色表示不同类型） + +  - 扫描线旋转效果 + +  - 近距离障碍物警告 + + + +五、静态工具函数 + +1\. get\_location\_bbox(location, camera) + +  - 功能：获取位置在相机中的边界框 + +  - 参数：location - 世界坐标，camera - 相机对象 + + + +2\. location\_to\_sensor\_cords(cords, location, sensor) + +  - 功能：将坐标转换为传感器坐标 + +  - 参数：cords - 坐标，location - 位置，sensor - 传感器 + + + +3\. location\_to\_world\_cords(cords, location) + +  - 功能：将坐标转换为世界坐标 + +  - 参数：cords - 坐标，location - 位置 + + + +4\. \_create\_vehicle\_bbox\_points(vehicle) + +  - 功能：创建车辆3D边界框点 + +  - 参数：vehicle - 车辆对象 + + + +5\. \_vehicle\_to\_sensor\_cords(cords, vehicle, sensor) + +  - 功能：将车辆边界框坐标转换为传感器坐标 + +  - 参数：cords - 坐标，vehicle - 车辆，sensor - 传感器 + + + +6\. \_vehicle\_to\_world\_cords(cords, vehicle) + +  - 功能：将车辆边界框坐标转换为世界坐标 + +  - 参数：cords - 坐标，vehicle - 车辆 + + + +7\. \_world\_to\_sensor\_cords(cords, sensor) + +  - 功能：将世界坐标转换为传感器坐标 + +  - 参数：cords - 坐标，sensor - 传感器 + + + +8\. get\_matrix(transform) + +  - 功能：从Carla变换创建矩阵 + +  - 参数：transform - Carla变换对象 + + + +六、使用说明 + +1\. 初始化：在main.py中通过PyGameDrawer(main)创建实例 + +2\. 调用：每帧调用相应的显示函数 + +3\. 坐标：大多数函数使用世界坐标，模块内部处理坐标转换 + +4\. 颜色：使用RGBA格式，支持透明度 + + + +七、可视化效果特性 + +1\. 颜色编码：根据数值大小使用不同颜色（绿/黄/橙/红） + +2\. 动画效果：闪烁、脉冲、渐变等 + +3\. 历史趋势：速度、评分等历史数据显示 + +4\. 实时更新：所有显示内容每帧更新 + +5\. 边界检查：确保绘制内容在屏幕内 + + + +================================================================================ + +""" From fc5bd77bc0f91b4101ab562f64a231370562cd04 Mon Sep 17 00:00:00 2001 From: 183899 Date: Sun, 28 Dec 2025 18:15:36 +0800 Subject: [PATCH 14/14] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E4=B8=80=E4=B8=AA?= =?UTF-8?q?=E7=8B=AC=E7=AB=8B=E7=9A=84MPC=E4=BB=A3=E7=A0=81=EF=BC=8C?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E6=9B=B4=E8=AF=A6=E7=BB=86=E7=9A=84=E6=8E=A7?= =?UTF-8?q?=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unmannedcar_MPC/mpc.py | 62 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 src/unmannedcar_MPC/mpc.py diff --git a/src/unmannedcar_MPC/mpc.py b/src/unmannedcar_MPC/mpc.py new file mode 100644 index 0000000000..d3ef8670ec --- /dev/null +++ b/src/unmannedcar_MPC/mpc.py @@ -0,0 +1,62 @@ +import math +import numpy as np +import utils +from scipy.optimize import minimize + + +class MPC: + HORIZON = 10 + L = 2.5 + + def __init__(self, drawer, ego): + self.drawer = drawer + self.ego = ego + + def run_step(self, path, speed_m_s, dt): + # convert path to body frame + path = utils.to_body_frame(self.ego, path) + + # fit the path with a polynomial + poly = np.polyfit(path[:, 0], path[:, 1], 3) + + # generate lane center locations + self.speed_dt = speed_m_s * dt + + x_arr = np.arange(1, self.HORIZON + 1) * self.speed_dt + self.locs = np.vstack((x_arr, np.polyval(poly, x_arr))).T + + # mpc + bounds = np.full((self.HORIZON, 2), (-0.3, 0.3)) + init_steer_arr = np.full(self.HORIZON, 0) + solution = minimize(self.objective, init_steer_arr, (), method='SLSQP', bounds=bounds, tol=1e-4) + eval_states = self.evaluate_states(solution.x) + + # draw lines + self.drawer.draw_camera_lines((255, 255, 255), utils.to_global_frame(self.ego, self.locs), 1) + self.drawer.draw_camera_lines((255, 0, 0), utils.to_global_frame(self.ego, np.array(eval_states)[:, 0:2]), 1) + + return solution.x[0] + + def mpc_model(self, state, steering): + x_t = state[0] + y_t = state[1] + psi_t = state[2] + + x_t_1 = x_t + self.speed_dt * np.cos(psi_t) + y_t_1 = y_t + self.speed_dt * np.sin(psi_t) + psi_t_1 = psi_t + self.speed_dt * np.tan(steering) / self.L + + return [x_t_1, y_t_1, psi_t_1] + + def evaluate_states(self, steer_arr): + states = [] + state = [0, 0, 0] + for steer in steer_arr: + state = self.mpc_model(state, steer) + states.append(state) + return states + + def objective(self, steer_arr, *args): + final_state = self.evaluate_states(steer_arr)[-1] + final_loc = self.locs[-1] + return math.sqrt((final_state[0] - final_loc[0]) ** 2 + (final_state[1] - final_loc[1]) ** 2) \ No newline at end of file