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