From 552d9bd33dd05487821a612bba951f7e459a2f8f Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 8 Dec 2025 10:13:45 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E5=9C=A8main=5Fhelp=5Ffunctions.py?= =?UTF-8?q?=E4=B8=AD=E6=B7=BB=E5=8A=A0=E4=B8=80=E4=B8=AA=E6=96=B0=E7=9A=84?= =?UTF-8?q?=E8=BD=A8=E8=BF=B9=E2=80=9C=E5=9C=86=E5=BD=A2=E8=BD=A8=E8=BF=B9?= =?UTF-8?q?=E2=80=9D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_help_functions.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py index 09ddbdb74b..17d78685a8 100644 --- a/src/Unmanned_vehicle_control/main_help_functions.py +++ b/src/Unmanned_vehicle_control/main_help_functions.py @@ -41,6 +41,24 @@ def get_eight_trajectory(x_init, y_init, total_points=100): theta_ref.append(theta_ref[-1]) return x_traj_rotated, y_traj_rotated, v_ref, theta_ref +def get_circle_trajectory(x_center, y_center, radius=25, total_points=200): + """生成圆形轨迹""" + t_values = np.linspace(0, 2 * np.pi, total_points) + x_traj = x_center + radius * np.cos(t_values) + y_traj = y_center + radius * np.sin(t_values) + + v_ref = [V_REF for _ in range(total_points)] + + # 计算参考角度（切线方向） + theta_ref = [] + for i in range(total_points - 1): + dx = x_traj[i + 1] - x_traj[i] + dy = y_traj[i + 1] - y_traj[i] + theta = np.arctan2(dy, dx) + theta_ref.append(theta) + theta_ref.append(theta_ref[-1]) # 最后一个点保持与前一个相同 + + return x_traj, y_traj, v_ref, theta_ref def get_ref_trajectory(x_traj, y_traj, theta_traj, current_idx): if current_idx + N < len(x_traj): From ffe1cbf3a651b94ba73ac9fbed53637813f84d07 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 15 Dec 2025 10:02:42 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0main=EF=BC=8C=E4=B8=BB?= =?UTF-8?q?=E5=87=BD=E6=95=B0=E6=96=B0=E6=B7=BB=E5=8A=A0=E8=B0=83=E7=94=A8?= =?UTF-8?q?=E5=9C=86=E5=BD=A2=E8=BD=A8=E8=BF=B9=EF=BC=8C=E5=B9=B6=E8=BF=90?= =?UTF-8?q?=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main.py | 42 +++++++++++++++++++++++----- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 8179babc2e..7d641fe115 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -1,10 +1,11 @@ import time import threading -from src.carla_simulator import CarlaSimulator +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.help_functions import get_eight_trajectory, get_ref_trajectory, update_reference_point -from src.logger import Logger -from src.mpc_controller import MpcController +from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_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: @@ -13,13 +14,40 @@ 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) carla.print_ego_vehicle_characteristics() carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) -logger = Logger() +#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]) +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_eight_trajectory(X_INIT_M, Y_INIT_M) +logger = Logger() current_idx = 0 laps = 0 From 7980325841b440bc859f5262f54a55748e909f8b Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 09:28:00 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E7=94=9F=E6=88=90=E4=BA=86=E6=96=B0?= =?UTF-8?q?=E7=9A=84=E8=9E=BA=E6=97=8B=E7=BA=BF=E8=BD=A8=E8=BF=B9=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E4=B8=94=E8=BD=A8=E8=BF=B9=E4=B8=8D=E8=B6=85=E5=87=BA?= =?UTF-8?q?=E5=9C=BA=E5=9C=B0=E8=8C=83=E5=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_help_functions.py | 33 ++++++++++++++++--- 1 file changed, 29 insertions(+), 4 deletions(-) diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py index 17d78685a8..ef4e8b76a0 100644 --- a/src/Unmanned_vehicle_control/main_help_functions.py +++ b/src/Unmanned_vehicle_control/main_help_functions.py @@ -1,7 +1,7 @@ import numpy as np from src.config import N, V_REF, A - +from src.config import CIRCLE_CENTER_X, CIRCLE_CENTER_Y, CIRCLE_RADIUS def update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=5.0): # 校验轨迹数组长度一致性与非空 if len(x_traj) != len(y_traj) or len(x_traj) == 0: @@ -41,14 +41,38 @@ def get_eight_trajectory(x_init, y_init, total_points=100): theta_ref.append(theta_ref[-1]) return x_traj_rotated, y_traj_rotated, v_ref, theta_ref -def get_circle_trajectory(x_center, y_center, radius=25, total_points=200): - """生成圆形轨迹""" +def get_circle_trajectory( + x_center=CIRCLE_CENTER_X, # 从config.py读取默认圆心X + y_center=CIRCLE_CENTER_Y, # 从config.py读取默认圆心Y + # radius=CIRCLE_RADIUS, # 从config.py读取默认半径 + radius=20, # 手动调节可适用无错误的半径 + total_points=200 +): + """生成圆形轨迹（默认参数从配置文件读取）""" t_values = np.linspace(0, 2 * np.pi, total_points) x_traj = x_center + radius * np.cos(t_values) y_traj = y_center + radius * np.sin(t_values) v_ref = [V_REF for _ in range(total_points)] + # 计算参考角度（切线方向） + theta_ref = [] + for i in range(total_points - 1): + dx = x_traj[i + 1] - x_traj[i] + dy = y_traj[i + 1] - y_traj[i] + theta = np.arctan2(dy, dx) + theta_ref.append(theta) + theta_ref.append(theta_ref[-1]) # 最后一个点保持与前一个相同 + + return x_traj, y_traj, v_ref, theta_ref +def get_spiral_trajectory(x_init, y_init, total_points=200, turns=2, scale=2): + """生成螺旋线轨迹""" + t_values = np.linspace(0, 2 * np.pi * turns, total_points) + # 极坐标方程：r = scale * t（半径随角度线性增加） + r = scale * t_values + x_traj = x_init + r * np.cos(t_values) + y_traj = y_init + r * np.sin(t_values) + v_ref = [V_REF for _ in range(total_points)] # 计算参考角度（切线方向） theta_ref = [] for i in range(total_points - 1): @@ -85,4 +109,5 @@ def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2): if denom > 0.01: return num / denom else: - return 0 \ No newline at end of file + return 0 +