Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 44 additions & 0 deletions src/Unmanned_vehicle_control/config.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
import numpy as np

X_INIT_M = 210 # Start X coordinate for vehicle
Y_INIT_M = 362 # Start Y coordinate for vehicle
A = 25 # Size of 8-trajectory
LAPS = 1 #Laps of simulation

MAX_ACCELERATION_M_S_2 = 10 # Vehicle characteristics (from CARLA simulator, see get_physics_control())
MAX_BRAKING_M_S_2 = -4.1 # Vehicle characteristics (from CARLA simulator, see get_physics_control())
MAX_WHEEL_ANGLE_RAD = 70 / 180 * np.pi # Vehicle characteristics (from CARLA simulator, see get_physics_control())
L = 2.875 #The wheelbase length of the vehicle (meters)

### MPC settings
N = 5 # Horizon of planning for MPC controller
dt = 0.1 #Sample time of control
MAX_CONTROL_WHEEL_ANGLE_RAD = 70 / 180 * np.pi # Maximum value which can use MPC controller in terms of steering
MAX_CONTROL_ACCELERATION_M_S_2 = 10 # Maximum value which can use MPC controller in terms of acceleration
MAX_CONTROL_BRAKING_M_S_2 = -4.1 # Maximum value which can use MPC controller in terms of braking
PATH_TOLERANCE_M = 0.5 # Acceptance radius of each waypoint
V_REF = 5 #Speed of vehicle

FINE_X_COEF = 10 #Fine for deviation from reference X coordinate
FINE_Y_COEF = 10 #Fine for deviation from reference Y coordinate
FINE_STEER_COEF = 0 #Fine for use high value of steering
FINE_ACC_COEF = 0 #Fine for use high values of acceleration
FINE_STEER_DOT_COEF = 100 #Fine for rapid steering
FINE_ACC_DOT_COEF = 1 #Fine for rapid changing of acceleration
FINE_V_COEF = 20 #Fine for deviation from reference speed
FINE_THETA_COEF = 0 #Fine for deviation from reference theta
FINE_LATERAL_COEF = 40 #Fine for deviation from path line
# 圆形轨迹参数
CIRCLE_CENTER_X = X_INIT_M # 圆心X坐标
CIRCLE_CENTER_Y = Y_INIT_M # 圆心Y坐标
CIRCLE_RADIUS = 50 # 圆半径

#添加
VISUALIZATION_CONFIG = {
'show_full_trajectory': False, # 是否显示完整轨迹
'show_perception_planning': True, # 是否显示感知规划信息
'show_frenet_frame': False, # 是否显示Frenet坐标系
'look_ahead_points': N, # 向前看的点数
'target_point_color': (255, 0, 0), # 目标点颜色 (R, G, B)
'trajectory_color': (0, 255, 0), # 轨迹线颜色
}
125 changes: 79 additions & 46 deletions src/Unmanned_vehicle_control/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,70 +3,36 @@
import numpy as np
from src.main_carla_simulator import CarlaSimulator
from src.config import X_INIT_M, Y_INIT_M, N, dt, V_REF, LAPS
from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, get_spiral_trajectory, update_reference_point
from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, \
get_spiral_trajectory, get_square_trajectory, update_reference_point
from src.main_logger import Logger
from src.main_mpc_controller import MpcController

"""
# 注释掉的轨迹绘制线程
def draw_trajectory_in_thread(carla, x_traj, y_traj, dt):
while True:
carla.draw_trajectory(x_traj, y_traj, height=0.2, green=255, life_time=dt * 2)
time.sleep(dt)
"""

carla = CarlaSimulator()
carla.load_world('Town02_Opt')
#carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) #“8”字形
carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) # "8"字形
carla.print_ego_vehicle_characteristics()
carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90)
"""
logger = Logger()
x_traj, y_traj, v_ref, theta_traj = get_eight_trajectory(X_INIT_M, Y_INIT_M) #“8”形状轨迹
current_idx = 0
laps = 0
"""
"""
# 圆形轨迹参数:圆心(X_INIT_M, Y_INIT_M),半径20米,200个点
x_traj, y_traj, v_ref, theta_traj = get_circle_trajectory() # 一行生成轨迹
init_x, init_y = x_traj[0], y_traj[0] # 2行获取初始位置
init_yaw = np.rad2deg(theta_traj[0]) # 1行获取初始角度
carla.spawn_ego_vehicle(
'vehicle.tesla.model3',
x=init_x,
y=init_y,
z=0.1,
yaw=init_yaw # 初始方向与轨迹一致
)

carla.print_ego_vehicle_characteristics()
# 调整 spectator 位置以便更好观察圆形轨迹
carla.set_spectator(X_INIT_M, Y_INIT_M, z=80, pitch=-90) # 从圆心正上方俯视
"""
#螺旋轨迹
x_traj, y_traj, v_ref, theta_traj = get_spiral_trajectory(
x_init=X_INIT_M,
y_init=Y_INIT_M,
turns=2, # 螺旋圈数
scale=2 # 螺旋缩放因子
) # 一行生成螺旋轨迹
init_x, init_y = x_traj[0], y_traj[0] # 获取初始位置
init_yaw = np.rad2deg(theta_traj[0]) # 获取初始角度
carla.spawn_ego_vehicle(
'vehicle.tesla.model3',
x=init_x,
y=init_y,
z=0.1,
yaw=init_yaw # 初始方向与轨迹一致
)

# 调整 spectator 位置以便观察螺旋轨迹
carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) # 降低高度,适应缩小的轨迹

logger = Logger()
x_traj, y_traj, v_ref, theta_traj = get_eight_trajectory(X_INIT_M, Y_INIT_M) # "8"形状轨迹
current_idx = 0
laps = 0

"""
# 注释掉的轨迹绘制线程
trajectory_thread = threading.Thread(target=draw_trajectory_in_thread, args=(carla, x_traj, y_traj, dt))
trajectory_thread.daemon = True
trajectory_thread.start()
"""

mpc_controller = MpcController(horizon=N, dt=dt)
try:
Expand All @@ -79,7 +45,12 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt):
mpc_controller.set_init_vehicle_state(x0, y0, theta0, v0)

x_ref, y_ref, theta_ref = get_ref_trajectory(x_traj, y_traj, theta_traj, current_idx)
carla.draw_trajectory(x_ref, y_ref, height=1.0, red=255, life_time=dt * 2)

# 使用感知规划绘制,只显示前方轨迹
carla.draw_perception_planning(x_ref, y_ref, current_idx=0, look_ahead_points=N)

# 绘制Frenet参考线(可选)
# carla.draw_frenet_frame(x_traj, y_traj, current_idx)

logger.log_controller_input(x0, y0, v0, theta0, x_ref[0], y_ref[0], V_REF, theta_ref[0])
mpc_controller.update_cost_function(x_ref, y_ref, theta_ref, v_ref)
Expand Down Expand Up @@ -111,4 +82,66 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt):
break
finally:
carla.clean()
logger.show_plots()
logger.show_plots()
"""
# 圆形轨迹参数:圆心(X_INIT_M, Y_INIT_M),半径20米,200个点
x_traj, y_traj, v_ref, theta_traj = get_circle_trajectory() # 一行生成轨迹
init_x, init_y = x_traj[0], y_traj[0] # 2行获取初始位置
init_yaw = np.rad2deg(theta_traj[0]) # 1行获取初始角度
carla.spawn_ego_vehicle(
'vehicle.tesla.model3',
x=init_x,
y=init_y,
z=0.1,
yaw=init_yaw # 初始方向与轨迹一致
)

carla.print_ego_vehicle_characteristics()
# 调整 spectator 位置以便更好观察圆形轨迹
carla.set_spectator(X_INIT_M, Y_INIT_M, z=80, pitch=-90) # 从圆心正上方俯视
"""
"""
#螺旋轨迹
x_traj, y_traj, v_ref, theta_traj = get_spiral_trajectory(
x_init=X_INIT_M,
y_init=Y_INIT_M,
turns=2, # 螺旋圈数
scale=2 # 螺旋缩放因子
) # 一行生成螺旋轨迹

init_x, init_y = x_traj[0], y_traj[0] # 获取初始位置
init_yaw = np.rad2deg(theta_traj[0]) # 获取初始角度
carla.spawn_ego_vehicle(
'vehicle.tesla.model3',
x=init_x,
y=init_y,
z=0.1,
yaw=init_yaw # 初始方向与轨迹一致
)

# 调整 spectator 位置以便观察螺旋轨迹
carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) # 降低高度,适应缩小的轨迹
"""
"""
# 使用新的方形轨迹
x_traj, y_traj, v_ref, theta_traj = get_square_trajectory(
x_init=X_INIT_M,
y_init=Y_INIT_M,
side_length=23, # 方形边长
total_points=200 # 轨迹点数量
)

# 设置初始位置和方向
init_x, init_y = x_traj[0], y_traj[0]
init_yaw = np.rad2deg(theta_traj[0])
carla.spawn_ego_vehicle(
'vehicle.tesla.model3',
x=init_x,
y=init_y,
z=0.1,
yaw=init_yaw # 初始方向与轨迹一致
)

# 调整 spectator 位置以便观察方形轨迹
carla.set_spectator(X_INIT_M + 20, Y_INIT_M + 20, z=60, pitch=-90) # 从方形中心上方俯视
"""
Loading