diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 7d641fe115..0973b4c355 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -3,7 +3,7 @@ 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, update_reference_point +from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, get_spiral_trajectory, update_reference_point from src.main_logger import Logger from src.main_mpc_controller import MpcController @@ -14,27 +14,20 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, 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) +#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 +""" +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( - x_center=X_INIT_M, - y_center=Y_INIT_M, - radius=20, - total_points=200 -) - -# 从圆形轨迹的第一个点生成车辆(确保车辆在轨迹上) -init_x, init_y = x_traj[0], y_traj[0] -# 获取初始角度(轨迹切线方向) -init_yaw = np.rad2deg(theta_traj[0]) +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, @@ -46,6 +39,26 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): 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() current_idx = 0