Skip to content
Merged
54 changes: 54 additions & 0 deletions src/unmannedcar_MPC/config.py
Original file line number Diff line number Diff line change
@@ -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, # 最大转向
}
266 changes: 204 additions & 62 deletions src/unmannedcar_MPC/main.py
Original file line number Diff line number Diff line change
@@ -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()
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
Loading