From 20e276a8b59910f31457b27368fd297e97f4c74b Mon Sep 17 00:00:00 2001 From: 183899 Date: Mon, 1 Dec 2025 11:44:13 +0800 Subject: [PATCH 1/4] =?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 2/4] =?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 3/4] =?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 4/4] =?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