Skip to content
Merged
Changes from all commits
Commits
Show all changes
44 commits
Select commit Hold shift + click to select a range
e84d4f5
提交main.py文件
Dec 18, 2025
a2350d9
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 18, 2025
938de18
增加第一人称视角
Dec 18, 2025
5f2b01e
增加第一视角
Dec 18, 2025
7f5525f
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
6032ef7
增加车辆状态信息显示
Dec 19, 2025
c849192
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
2218a0f
增加自主控制与路径跟随
Dec 19, 2025
f558ec6
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
b1a8426
解决了多个入口脚本问题
Dec 19, 2025
46fca0d
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
0443fc2
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
ff2f353
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
9768bf1
增加自主控制与路径跟随
Dec 19, 2025
7bbdf3d
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
e4c440b
实现弯道自适应转向
Dec 19, 2025
c4bcb26
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
85e2c10
实现弯道自适应转向
Dec 19, 2025
8f9ab62
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 19, 2025
51bb9b3
实现车辆沿道路的自主航点跟踪
Dec 19, 2025
b548d08
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 20, 2025
3edfc11
实现车辆自主道路航点跟随
Dec 20, 2025
aaedf77
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 20, 2025
9c79eec
增加避障控制
Dec 20, 2025
939dc5f
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 21, 2025
3286110
增加了交通信号灯响应
Dec 21, 2025
2543c53
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 21, 2025
914f120
多车辆协同控制
Dec 21, 2025
cee1f83
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 21, 2025
63165f8
更新README.md
Dec 21, 2025
f87a00c
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 21, 2025
38a6d8b
更新README.md
Dec 21, 2025
4c71825
更新README.md
Dec 21, 2025
9c8154d
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 22, 2025
17f2b0c
进行ros封装
Dec 22, 2025
585f250
上传功能包
Dec 22, 2025
6b6d930
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 22, 2025
7b57177
上传ros功能包
Dec 22, 2025
53c0bd9
删除ros目录下的package.xml和CMakeLists.txt文件
Dec 22, 2025
684015a
上传功能包
Dec 22, 2025
56ad1a5
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 22, 2025
c5bee8a
更新README.md
Dec 22, 2025
74ea779
Merge branch 'OpenHUTB:main' into main
Liyang2302 Dec 23, 2025
7638494
添加ACC跟车相关的全局配置
Dec 23, 2025
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
74 changes: 59 additions & 15 deletions src/autonomous_driving_car/main.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
CARLA 多车辆协同控制版:修复出生点索引越界问题
CARLA 多车辆协同控制版:V1.0 增强ACC跟车+紧急避障
"""

import sys
Expand Down Expand Up @@ -52,6 +52,12 @@
PID_KI = 0.01
PID_KD = 0.02

# ACC跟车配置(新增)
SAFE_TIME_GAP = 1.5 # 安全时距(秒)
MIN_SAFE_DISTANCE = 5.0 # 最小安全距离(米)
EMERGENCY_DECEL_RATE = 5.0 # 紧急制动减速度(km/h/帧)
LEAD_BRAKE_THRESHOLD = -10.0 # 前车急刹加速度阈值(km/h/s)

# 交通规则配置
TRAFFIC_LIGHT_STOP_DISTANCE = 4.0
TRAFFIC_LIGHT_DETECTION_RANGE = 50.0
Expand All @@ -72,12 +78,13 @@
# 全局变量
current_view_vehicle_id = 1
vehicle_agents = []
COLLISION_FLAG = {} # 碰撞标志(新增)

# 日志配置
logging.basicConfig(
level=logging.INFO,
format="%(asctime)s - 车辆%(vehicle_id)s - %(levelname)s - %(message)s",
handlers=[logging.FileHandler("multi_vehicle_simulation.log"), logging.StreamHandler()]
handlers=[logging.FileHandler("multi_vehicle_simulation_v1.log"), logging.StreamHandler()]
)

# ===================== 核心工具函数 ======================
Expand Down Expand Up @@ -326,6 +333,11 @@ def __init__(self, world, map, vehicle_id, spawn_point, vehicle_model, base_spee
self.logger = logging.getLogger(__name__)
self.logger = logging.LoggerAdapter(self.logger, {"vehicle_id": vehicle_id})

# 新增:ACC跟车相关属性
self.last_lead_speed = 0.0 # 前车上次速度
self.stuck_count = 0 # 卡死计数
COLLISION_FLAG[self.vehicle_id] = False # 初始化碰撞标志

# 生成车辆
self.vehicle_bp = self.world.get_blueprint_library().find(vehicle_model)
if self.vehicle_bp.has_attribute("color"):
Expand Down Expand Up @@ -400,17 +412,42 @@ def update(self):
base_target_speed = self.base_speed * speed_factor
base_target_speed = max(8.0, base_target_speed)

# 跟车控制
global vehicle_agents
# ========== 新增:精细化ACC跟车+紧急避障逻辑 ==========
if self.vehicle_id > 1 and len(vehicle_agents) >= self.vehicle_id:
try:
lead_vehicle = vehicle_agents[self.vehicle_id - 2].vehicle
lead_agent = vehicle_agents[self.vehicle_id - 2]
lead_vehicle = lead_agent.vehicle
lead_vehicle_transform = lead_vehicle.get_transform()

# 计算前车速度和加速度
lead_vel = lead_vehicle.get_velocity()
lead_speed = math.hypot(lead_vel.x, lead_vel.y) * 3.6
lead_acc = (lead_speed - lead_agent.last_lead_speed) / 0.03 # 30Hz刷新率,计算加速度
lead_agent.last_lead_speed = lead_speed # 更新前车上次速度

# 计算安全跟车距离(安全时距+最小安全距)
safe_dist = (current_speed / 3.6) * SAFE_TIME_GAP + MIN_SAFE_DISTANCE
dist_to_lead = vehicle_transform.location.distance(lead_vehicle_transform.location)
if dist_to_lead < 15.0:
base_target_speed = max(5.0, base_target_speed * 0.5)
except:
pass

# 动态调整目标速度
if dist_to_lead < safe_dist - 2:
# 过近:减速至前车速度-2(不低于5km/h)
base_target_speed = max(5.0, lead_speed - 2)
elif dist_to_lead > safe_dist + 2:
# 过远:加速至前车速度+2(不超基础速度)
base_target_speed = min(self.base_speed * speed_factor, lead_speed + 2)
else:
# 安全距离:与前车速度同步
base_target_speed = lead_speed

# 紧急避障:前车急刹(加速度<阈值)
if lead_acc < LEAD_BRAKE_THRESHOLD:
base_target_speed = max(0.0, current_speed - EMERGENCY_DECEL_RATE)
self.logger.warning(f"前车急刹(加速度{lead_acc:.1f}km/h/s)!紧急减速至{base_target_speed:.1f}km/h")

except Exception as e:
self.logger.warning(f"ACC跟车计算异常:{e}")
# ========== ACC跟车逻辑结束 ==========

# 交通灯处理
target_speed, traffic_light_status = self.traffic_light_manager.handle_traffic_light_logic(
Expand All @@ -426,17 +463,24 @@ def update(self):
throttle = 0.0
brake = 1.0

# 新增:碰撞后紧急停车
if COLLISION_FLAG.get(self.vehicle_id, False):
throttle = 0.0
brake = 1.0
self.logger.error("检测到碰撞,紧急停车!")

# 应用控制
control = carla.VehicleControl()
control.steer = steer
control.throttle = throttle
control.brake = brake
self.vehicle.apply_control(control)

# 日志输出
# 日志输出(新增ACC相关信息)
self.logger.info(
f"速度:{current_speed:5.1f}km/h | 目标:{target_speed:5.1f} | "
f"弯道:{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态:{traffic_light_status}"
f"弯道:{['直道', '缓弯', '急弯'][curve_level]:<3} | 灯状态:{traffic_light_status} | "
f"ACC:{'激活' if self.vehicle_id>1 else '未激活'}"
)

return True
Expand Down Expand Up @@ -521,10 +565,10 @@ def calculate(self, target_speed, current_speed):
p = self.kp * error
self.integral += self.ki * error
self.integral = np.clip(self.integral, -1.0, 1.0)
i = self.integral
d = self.kd * (error - self.last_error)
self.last_error = error
return np.clip(p + i + d, 0.0, 1.0)
# 修复:将 i 改为 self.integral
return np.clip(p + self.integral + d, 0.0, 1.0)

class TrafficLightManager:
def __init__(self, vehicle_id):
Expand Down Expand Up @@ -681,7 +725,7 @@ def main():
global current_view_vehicle_id, vehicle_agents
pygame.init()
screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
pygame.display.set_caption(f"CARLA多车辆视角({VEHICLE_COUNT}辆车)- 按1/2/3切换视角,按S切换分屏,按V切换俯视视角")
pygame.display.set_caption(f"CARLA多车辆视角({VEHICLE_COUNT}辆车)- V1.0 ACC跟车 - 按1/2/3切换视角,按S切换分屏,按V切换俯视视角")

client = None
world = None
Expand Down Expand Up @@ -777,7 +821,7 @@ def cleanup():
if len(vehicle_agents) == 0:
raise RuntimeError("无车辆生成成功,仿真终止")

print(f"\n共生成{len(vehicle_agents)}辆车辆!")
print(f"\n共生成{len(vehicle_agents)}辆车辆!V1.0 ACC跟车功能已启用")

# 创建全局俯视相机
try:
Expand Down
Loading