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/5] =?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/5] =?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/5] =?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 + From a04deab7342dca0c846d91fd189c9be2ffb3eff0 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 10:12:27 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E6=9B=B4=E6=96=B0main.py=EF=BC=8C=E5=B9=B6?= =?UTF-8?q?=E8=B0=83=E7=94=A8=E8=9E=BA=E6=97=8B=E8=BD=A8=E8=BF=B9=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E8=BF=90=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main.py | 51 +++++++++++++++++----------- 1 file changed, 32 insertions(+), 19 deletions(-) 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 From a645213666ccebf6864c117275e371eec2df603f Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 11:31:13 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E5=90=AF=E5=8A=A8carla?= =?UTF-8?q?=E6=96=87=E4=BB=B6=EF=BC=8C=E5=88=9D=E5=A7=8B=E5=8C=96=E5=9C=B0?= =?UTF-8?q?=E5=9B=BE=E5=9C=BA=E6=99=AF=E7=AD=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_carla_simulator.py | 105 ++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 src/Unmanned_vehicle_control/main_carla_simulator.py diff --git a/src/Unmanned_vehicle_control/main_carla_simulator.py b/src/Unmanned_vehicle_control/main_carla_simulator.py new file mode 100644 index 0000000000..729af0edad --- /dev/null +++ b/src/Unmanned_vehicle_control/main_carla_simulator.py @@ -0,0 +1,105 @@ +import carla +import numpy as np + +from src.config import MAX_BRAKING_M_S_2, MAX_WHEEL_ANGLE_RAD, MAX_ACCELERATION_M_S_2 + + +class CarlaSimulator: + def __init__(self): + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + self.ego_vehicle = None + for vehicle in self.world.get_actors().filter('*vehicle*'): + vehicle.destroy() + + def load_world(self, map_name): + self.client.load_world(map_name) + + def spawn_ego_vehicle(self, vehicle_name, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): + blueprint_library = self.world.get_blueprint_library() + vehicle_bp = blueprint_library.filter(vehicle_name)[0] + spawn_location = carla.Location(x, y, z) + spawn_rotation = carla.Rotation(pitch, yaw,roll) + spawn_transform = carla.Transform(location=spawn_location, rotation=spawn_rotation) + self.ego_vehicle = self.world.spawn_actor(vehicle_bp, spawn_transform) + + def set_spectator(self, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): + spectator = self.world.get_spectator() + location = carla.Location(x=x, y=y, z=z) + rotation = carla.Rotation(pitch=pitch, yaw=yaw, roll=roll) + spectator_transform = carla.Transform(location, rotation) + spectator.set_transform(spectator_transform) + + def clean(self): + for vehicle in self.world.get_actors().filter('*vehicle*'): + vehicle.destroy() + + def draw_trajectory(self, x_traj, y_traj, height = 0, thickness = 0.1, red = 0, green = 0, blue = 0, life_time = 0.1): + for i in range(len(x_traj) - 1): + start_point = carla.Location(x=x_traj[i], y=y_traj[i], z=height) + end_point = carla.Location(x=x_traj[i + 1], y=y_traj[i + 1], z=height) + self.world.debug.draw_line(start_point, end_point, thickness=thickness, color=carla.Color(red, green, blue), life_time=life_time) + + def get_main_ego_vehicle_state(self): + transform = self.ego_vehicle.get_transform() + x = transform.location.x + y = transform.location.y + theta = np.deg2rad(transform.rotation.yaw) + v = np.sqrt(self.ego_vehicle.get_velocity().x**2 + self.ego_vehicle.get_velocity().y**2) + return x, y, theta, v + + def apply_control(self, steer, throttle, brake): + self.ego_vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer, brake=brake)) + + @staticmethod + def process_control_inputs(wheel_angle_rad, acceleration_m_s_2): + if acceleration_m_s_2 == 0: + throttle = 0 + brake = 0 + elif acceleration_m_s_2 < 0: + throttle = 0 + brake = acceleration_m_s_2 / MAX_BRAKING_M_S_2 + else: + throttle = acceleration_m_s_2 / MAX_ACCELERATION_M_S_2 + brake = 0 + steer = wheel_angle_rad / MAX_WHEEL_ANGLE_RAD + return throttle, brake, steer + + def print_ego_vehicle_characteristics(self): + if not self.ego_vehicle: + print("Vehicle not spawned yet!") + return None + + physics_control = self.ego_vehicle.get_physics_control() + + print("Vehicle Physics Information.\n") + + print("Wheel Information:") + for i, wheel in enumerate(physics_control.wheels): + print(f" Wheel {i + 1}:") + print(f" Tire Friction: {wheel.tire_friction}") + print(f" Damping Rate: {wheel.damping_rate}") + print(f" Max Steer Angle: {wheel.max_steer_angle}") + print(f" Radius: {wheel.radius}") + print(f" Max Brake Torque: {wheel.max_brake_torque}") + print(f" Max Handbrake Torque: {wheel.max_handbrake_torque}") + print(f" Position (x, y, z): ({wheel.position.x}, {wheel.position.y}, {wheel.position.z})") + + print(f" Torque Curve:") + for point in physics_control.torque_curve: + print(f"RPM: {point.x}, Torque: {point.y}") + print(f" Max RPM: {physics_control.max_rpm}") + print(f" MOI (Moment of Inertia): {physics_control.moi}") + print(f" Damping Rate Full Throttle: {physics_control.damping_rate_full_throttle}") + print(f" Damping Rate Zero Throttle Clutch Engaged: {physics_control.damping_rate_zero_throttle_clutch_engaged}") + print(f" Damping Rate Zero Throttle Clutch Disengaged: {physics_control.damping_rate_zero_throttle_clutch_disengaged}") + print(f" If True, the vehicle will have an automatic transmission: {physics_control.use_gear_autobox}") + print(f" Gear Switch Time: {physics_control.gear_switch_time}") + print(f" Clutch Strength: {physics_control.clutch_strength}") + print(f" Final Ratio: {physics_control.final_ratio}") + print(f" Mass: {physics_control.mass}") + print(f" Drag coefficient: {physics_control.drag_coefficient}") + print(f" Steering Curve:") + for point in physics_control.steering_curve: + print(f"Speed: {point.x}, Steering: {point.y}") \ No newline at end of file