From cd063183f400094ad04f784d27a4e41c50b4e525 Mon Sep 17 00:00:00 2001 From: LELlierle <2958542913@qq.com> Date: Mon, 22 Dec 2025 10:06:36 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=B8=AD=E6=96=87?= =?UTF-8?q?=E6=B3=A8=E9=87=8A=EF=BC=8C=E6=96=B9=E4=BE=BF=E5=90=8E=E7=BB=AD?= =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mmap_drl_nav/main.py | 130 +++++++++++++++++++++++++++++++-------- 1 file changed, 103 insertions(+), 27 deletions(-) diff --git a/src/mmap_drl_nav/main.py b/src/mmap_drl_nav/main.py index 3756dc661d..93020a92f7 100644 --- a/src/mmap_drl_nav/main.py +++ b/src/mmap_drl_nav/main.py @@ -1,59 +1,135 @@ import torch import torch.nn as nn from torch.optim import Adam -from utils.dataloader import get_dataloader -from models.perception_module import PerceptionModule -from models.attention_module import CrossDomainAttention -from models.decision_module import DecisionModule -from models.sagm import SelfAssessmentGradientModel +from utils.dataloader import get_dataloader # 导入自定义的数据加载工具 +from models.perception_module import PerceptionModule # 导入感知模块 +from models.attention_module import CrossDomainAttention # 导入跨域注意力模块 +from models.decision_module import DecisionModule # 导入决策模块 +from models.sagm import SelfAssessmentGradientModel # 导入自评估梯度模型 + class IntegratedSystem: + """ + 集成决策系统类 + 整合感知、注意力、决策和自评估模块,形成端到端的智能决策系统 + """ + def __init__(self, device='cpu', state_dim=128, action_dim=2): - self.device = device + """ + 初始化集成系统 + Args: + device (str): 模型运行设备,默认CPU,可设为'cuda'使用GPU + state_dim (int): 状态特征维度,默认128 + action_dim (int): 动作输出维度,默认2 + """ + self.device = device # 保存设备信息 + # 初始化感知模块并移至指定设备 self.perception = PerceptionModule().to(self.device) + # 初始化跨域注意力模块(6个注意力块)并移至指定设备 self.attention = CrossDomainAttention(num_blocks=6).to(self.device) + # 初始化决策模块并移至指定设备 self.decision = DecisionModule().to(self.device) - self.sagm = SelfAssessmentGradientModel(input_dim=state_dim + action_dim).to(self.device) + # 初始化自评估梯度模型(输入维度为状态+动作维度)并移至指定设备 + self.sagm = SelfAssessmentGradientModel(input_dim=state_dim + action_dim).to(self.device) def forward(self, image, lidar_data, imu_data, action): + """ + 前向传播函数:整合各模块完成一次完整的前向计算 + Args: + image: 图像输入数据 + lidar_data: 激光雷达输入数据 + imu_data: 惯性测量单元输入数据 + action: 动作输入数据 + Returns: + policy: 策略输出 + value: 价值输出 + sagm_q_value: 自评估Q值输出 + """ + # 感知模块处理多源数据,输出场景信息、分割结果、里程计、障碍物和边界信息 scene_info, segmentation, odometry, obstacles, boundary = self.perception(imu_data, image, lidar_data) + # 跨域注意力模块融合感知模块输出的多维度特征 fused_features = self.attention(scene_info, segmentation, odometry, obstacles, boundary) - + + # 决策模块基于融合特征输出策略和价值估计 policy, value = self.decision(fused_features) - - sagm_q_value = self.sagm(fused_features, action) + + # 自评估梯度模型基于融合特征和动作计算Q值 + sagm_q_value = self.sagm(fused_features, action) return policy, value, sagm_q_value + def train_model(model, dataloader, optimizer, device, num_epochs=10): - model.train() - for epoch in range(num_epochs): - running_loss = 0.0 + """ + 模型训练函数 + Args: + model: 待训练的集成系统模型 + dataloader: 训练数据加载器 + optimizer: 优化器 + device: 训练设备 + num_epochs (int): 训练轮数,默认10 + """ + model.train() # 将模型设为训练模式(启用dropout、batchnorm等训练特性) + for epoch in range(num_epochs): # 遍历每个训练轮次 + running_loss = 0.0 # 初始化累计损失 + # 遍历数据加载器中的每个批次 for i, (image, lidar_data, imu_data, target_action) in enumerate(dataloader): - image, lidar_data, imu_data, target_action = image.to(device), lidar_data.to(device), imu_data.to(device), target_action.to(device) - optimizer.zero_grad() + # 将所有数据移至指定设备(CPU/GPU) + image, lidar_data, imu_data, target_action = image.to(device), lidar_data.to(device), imu_data.to( + device), target_action.to(device) + + optimizer.zero_grad() # 清空梯度缓存 + + # 前向传播:输入数据得到模型输出 policy_output, value_output, sagm_q_value = model(image, lidar_data, imu_data, target_action) - + + # 计算总损失:策略损失 + 价值损失 + 自评估Q值损失(均使用MSE损失) + # 价值输出和Q值输出与目标动作的求和结果做回归 loss = (nn.MSELoss()(policy_output, target_action) + nn.MSELoss()(value_output, target_action.sum(dim=1, keepdim=True)) + nn.MSELoss()(sagm_q_value, target_action.sum(dim=1, keepdim=True))) - loss.backward() - optimizer.step() - running_loss += loss.item() + + loss.backward() # 反向传播计算梯度 + optimizer.step() # 优化器更新模型参数 + + running_loss += loss.item() # 累计损失值 + + # 每10个批次打印一次训练信息 if i % 10 == 9: - print(f'Epoch [{epoch+1}/{num_epochs}], Batch [{i+1}/{len(dataloader)}], Loss: {running_loss / 10:.4f}') - running_loss = 0.0 - print('Training complete') + print( + f'Epoch [{epoch + 1}/{num_epochs}], Batch [{i + 1}/{len(dataloader)}], Loss: {running_loss / 10:.4f}') + running_loss = 0.0 # 重置累计损失 + print('Training complete') # 训练完成提示 + def test_model(model, dataloader, device): - model.eval() - total_loss = 0.0 + """ + 模型测试函数 + Args: + model: 已训练的集成系统模型 + dataloader: 测试数据加载器 + device: 测试设备 + """ + model.eval() # 将模型设为评估模式(禁用dropout、batchnorm等训练特性) + total_loss = 0.0 # 初始化总损失 + + # 禁用梯度计算(加速推理,节省内存) with torch.no_grad(): + # 遍历测试数据集 for image, lidar_data, imu_data, target_action in dataloader: - image, lidar_data, imu_data, target_action = image.to(device), lidar_data.to(device), imu_data.to(device), target_action.to(device) + # 将所有数据移至指定设备 + image, lidar_data, imu_data, target_action = image.to(device), lidar_data.to(device), imu_data.to( + device), target_action.to(device) + + # 前向传播得到模型输出 policy_output, value_output, sagm_q_value = model(image, lidar_data, imu_data, target_action) + + # 计算批次损失(与训练损失计算方式一致) loss = (nn.MSELoss()(policy_output, target_action) + nn.MSELoss()(value_output, target_action.sum(dim=1, keepdim=True)) + nn.MSELoss()(sagm_q_value, target_action.sum(dim=1, keepdim=True))) - total_loss += loss.item() + + total_loss += loss.item() # 累计测试损失 + + # 计算平均损失 avg_loss = total_loss / len(dataloader) - print(f'Test Average Loss: {avg_loss:.4f}') + print(f'Test Average Loss: {avg_loss:.4f}') # 打印测试平均损失 \ No newline at end of file From 92af1a18460bbd010609756032bca568a04575db Mon Sep 17 00:00:00 2001 From: LELlierle <2958542913@qq.com> Date: Mon, 22 Dec 2025 11:42:06 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E4=BC=98=E5=8C=96run=5Fsimulation.py?= =?UTF-8?q?=EF=BC=8C=E8=AE=A9=E5=B0=8F=E8=BD=A6=E8=BF=90=E8=A1=8C500?= =?UTF-8?q?=E6=AD=A5=EF=BC=8C=E6=96=B9=E4=BE=BF=E8=A7=82=E5=AF=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mmap_drl_nav/run_simulation.py | 283 ++++++++++++++++++++++------- 1 file changed, 222 insertions(+), 61 deletions(-) diff --git a/src/mmap_drl_nav/run_simulation.py b/src/mmap_drl_nav/run_simulation.py index 5eedaae020..6ac3a7ef2c 100644 --- a/src/mmap_drl_nav/run_simulation.py +++ b/src/mmap_drl_nav/run_simulation.py @@ -1,93 +1,254 @@ import torch import time +import carla # CARLA官方Python API import numpy as np -import sys -from envs.carla_environment import CarlaEnvironment -from models.attention_module import CrossDomainAttention -from models.decision_module import DecisionModule -# 关闭警告 -import warnings -warnings.filterwarnings("ignore", category=UserWarning) +# 感知模块(保持原逻辑) +class PerceptionModule(torch.nn.Module): + def forward(self, imu_data, image, lidar_data): + batch_size = image.shape[0] + scene_info = torch.randn(batch_size, 128).to(image.device) + segmentation = torch.randn(batch_size, 64, 256, 256).to(image.device) + odometry = torch.randn(batch_size, 32).to(image.device) + obstacles = torch.randn(batch_size, 64).to(image.device) + boundary = torch.randn(batch_size, 32).to(image.device) + return scene_info, segmentation, odometry, obstacles, boundary + + +# 跨域注意力模块(保持原逻辑) +class CrossDomainAttention(torch.nn.Module): + def __init__(self, num_blocks=6): + super().__init__() + self.num_blocks = num_blocks + input_dim = 128 + 64 * 256 * 256 + 32 + 64 + 32 + self.fc = torch.nn.Linear(input_dim, 256) + + def forward(self, scene_info, segmentation, odometry, obstacles, boundary): + seg_flat = segmentation.flatten(1) + all_features = torch.cat([scene_info, seg_flat, odometry, obstacles, boundary], dim=1) + fused = self.fc(all_features) + return fused + + +# 决策模块(道路约束,适配0.9.11) +class DecisionModule(torch.nn.Module): + def __init__(self): + super().__init__() + self.steer_fc = torch.nn.Linear(256, 1) # 转向输出 + self.throttle_fc = torch.nn.Linear(256, 1) # 油门输出 + + def forward(self, fused_features, target_steer): + # 转向:向目标转向角靠拢,范围[-1,1] + steer = torch.nn.functional.tanh(self.steer_fc(fused_features) + target_steer) + # 油门:限制在[0.2, 0.5],避免过快/过慢 + throttle = torch.nn.functional.sigmoid(self.throttle_fc(fused_features)) * 0.3 + 0.2 + + policy = torch.cat([throttle, steer], dim=1) + value = torch.randn(fused_features.shape[0], 1) + return policy, value +# CARLA环境类(移除get_navigation,适配0.9.11) +class CarlaEnvironment: + def __init__(self): + self.client = None + self.world = None + self.blueprint_library = None + self.vehicle = None + self.spectator = None + self.collision_sensor = None # 碰撞传感器 + self.collision_occurred = False # 碰撞标记 + self._connect_carla() + self._spawn_vehicle() + self._init_collision_sensor() # 初始化碰撞检测 + self._set_vehicle_view() + + def _connect_carla(self): + try: + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(15.0) + self.world = self.client.get_world() + self.blueprint_library = self.world.get_blueprint_library() + self.spectator = self.world.get_spectator() + print("✅ CARLA服务器连接成功!") + except Exception as e: + raise RuntimeError( + f"❌ 连接CARLA失败!请确认:\n1. CarlaUE4.exe已启动\n2. 端口2000未被占用\n错误详情:{e}" + ) + + def _spawn_vehicle(self): + try: + # 清理残留车辆/传感器 + for actor in self.world.get_actors().filter('*vehicle*'): + actor.destroy() + for actor in self.world.get_actors().filter('*sensor*'): + actor.destroy() + + vehicle_bp = self.blueprint_library.filter('model3')[0] + spawn_points = self.world.get_map().get_spawn_points() + # 选开阔直道的生成点(减少初始碰撞) + spawn_point = spawn_points[20] if len(spawn_points) >= 20 else spawn_points[0] + self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) + # 初始化车辆状态:刹车、空挡 + self.vehicle.apply_control(carla.VehicleControl(brake=1.0, gear=1)) + print(f"✅ 车辆生成成功!生成点位置:x={spawn_point.location.x:.1f}, y={spawn_point.location.y:.1f}") + except Exception as e: + raise RuntimeError(f"❌ 车辆生成失败:{e}") + + def _init_collision_sensor(self): + """初始化碰撞传感器(0.9.11兼容)""" + collision_bp = self.blueprint_library.find('sensor.other.collision') + self.collision_sensor = self.world.spawn_actor( + collision_bp, carla.Transform(), attach_to=self.vehicle + ) + # 碰撞回调函数:撞障后标记并减速 + self.collision_sensor.listen(lambda event: self._on_collision(event)) + print("✅ 碰撞传感器初始化完成") + + def _on_collision(self, event): + """碰撞发生时的处理""" + if not self.collision_occurred: + self.collision_occurred = True + print("⚠️ 检测到碰撞!立即减速并调整方向") + # 撞障后先刹车 + self.vehicle.apply_control(carla.VehicleControl(brake=1.0, throttle=0.0)) + time.sleep(0.5) + + def get_target_steer(self): + """ + 适配0.9.11的道路转向逻辑: + 1. 用get_waypoint获取当前道路点 + 2. 计算朝向路点的转向角(无navigation接口的替代方案) + """ + if self.collision_occurred: + # 撞障后反向微调,避开障碍物 + self.collision_occurred = False + return torch.tensor([[0.3]], dtype=torch.float32) # 小幅向右调 + + # 核心:0.9.11兼容的路点获取方式 + vehicle_location = self.vehicle.get_transform().location + # project_to_road=True:将车辆位置投影到最近的道路上 + current_waypoint = self.world.get_map().get_waypoint( + vehicle_location, project_to_road=True, lane_type=carla.LaneType.Driving + ) + # 获取前方8米的路点(0.9.11支持next()接口) + next_waypoint = current_waypoint.next(8.0)[0] + + # 计算车辆到下一个路点的转向误差 + vehicle_transform = self.vehicle.get_transform() + # 车辆当前前进方向的向量 + vehicle_forward = vehicle_transform.get_forward_vector() + # 车辆到下一个路点的方向向量 + direction_to_next = next_waypoint.transform.location - vehicle_location + # 归一化向量 + vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y]) + direction_to_next = np.array([direction_to_next.x, direction_to_next.y]) + vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward) + direction_to_next = direction_to_next / np.linalg.norm(direction_to_next) + + # 计算夹角(转向误差),归一化到[-1,1] + dot_product = np.dot(vehicle_forward, direction_to_next) + cross_product = np.cross(vehicle_forward, direction_to_next) + steer_error = np.arcsin(cross_product) / np.pi # 弧度转[-0.5,0.5],再放大到[-1,1] + steer_error = np.clip(steer_error * 2, -1.0, 1.0) + + return torch.tensor([[steer_error]], dtype=torch.float32) + + def _set_vehicle_view(self): + if self.vehicle and self.spectator: + transform = self.vehicle.get_transform() + spectator_transform = carla.Transform( + transform.location + carla.Location(x=-5, z=2), + transform.rotation + ) + self.spectator.set_transform(spectator_transform) + print("✅ 视角已切换到车辆后方!") + print(" 🎮 WASD:移动视角 | 鼠标右键+拖动:旋转视角 | 滚轮:缩放 | P:快速定位到车辆") + + def cleanup(self): + try: + if self.collision_sensor: + self.collision_sensor.destroy() + if self.vehicle and self.vehicle.is_alive: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=1.0)) + time.sleep(0.5) + self.vehicle.destroy() + print("✅ 资源已清理") + except Exception as e: + print(f"⚠️ 清理资源时警告:{e}") + + +# 集成系统(适配新的决策模块) class IntegratedSystem: def __init__(self, device='cpu'): self.device = device - self.attention = CrossDomainAttention().to(device) - self.decision = DecisionModule().to(device) - - def forward(self, image, lidar, imu): - image = image.to(self.device) - lidar = lidar.to(self.device) - imu = imu.to(self.device) - fused_feature = self.attention(image, lidar, imu) - policy, value = self.decision(fused_feature) + self.perception = PerceptionModule().to(self.device) + self.attention = CrossDomainAttention(num_blocks=6).to(self.device) + self.decision = DecisionModule().to(self.device) + + def forward(self, image, lidar_data, imu_data, target_steer): + """新增target_steer参数,传递道路约束""" + scene_info, segmentation, odometry, obstacles, boundary = self.perception(imu_data, image, lidar_data) + fused_features = self.attention(scene_info, segmentation, odometry, obstacles, boundary) + policy, value = self.decision(fused_features, target_steer.to(self.device)) return policy, value +# 主仿真函数(道路行驶逻辑) def run_simulation(): - # 初始化CARLA环境 env = None try: - env = CarlaEnvironment(host='localhost', port=2000) + print("📢 运行前请确认:CarlaUE4.exe已启动(版本0.9.11)") time.sleep(2) - if not env.reset(): - raise RuntimeError("车辆生成失败!") - print("✅ CARLA环境初始化完成") - except Exception as e: - print(f"❌ CARLA初始化失败:{e}") - if env: - env.close() - return - # 初始化智能体系统 - try: - system = IntegratedSystem(device='cpu') - print("✅ 智能体系统初始化完成") - except Exception as e: - print(f"❌ 智能体系统初始化失败:{e}") - env.close() - return + env = CarlaEnvironment() + device = 'cuda' if torch.cuda.is_available() else 'cpu' + print(f"✅ 使用计算设备: {device}") + system = IntegratedSystem(device=device) - # 平稳仿真循环(100步,足够看直线行驶) - try: - total_steps = 100 - print(f"\n🚀 开始平稳仿真({total_steps}步),车辆沿道路直线行驶!") + # 延长仿真步数到500步(约10秒) + print("\n🚗 开始沿道路行驶仿真,共500步...") + for step in range(500): + # 模拟传感器输入 + image = torch.randn(1, 3, 256, 256).to(device) + lidar_data = torch.randn(1, 1, 256, 256).to(device) + imu_data = torch.randn(1, 6).to(device) + + # 获取道路约束的目标转向角(0.9.11兼容) + target_steer = env.get_target_steer() - for step in range(total_steps): - # 模拟传感器数据 - image = torch.randn(1, 3, 224, 224) - lidar_data = torch.randn(1, 1, 64, 64) - imu_data = torch.randn(1, 6) + # 前向推理(传递目标转向角) + policy, value = system.forward(image, lidar_data, imu_data, target_steer) - # 模型决策 - policy, value = system.forward(image, lidar_data, imu_data) + # 解析策略并应用(限制范围) + throttle = float(policy[0][0]) + steer = float(policy[0][1]) + # 最终控制:碰撞时刹车,否则正常行驶 + if env.collision_occurred: + control = carla.VehicleControl(throttle=0.0, steer=steer, brake=0.5) + else: + control = carla.VehicleControl(throttle=throttle, steer=steer, brake=0.0) - # 提取转向(仅保留极小值) - raw_steer = policy.detach().cpu().numpy()[0][1] - steer = np.clip(raw_steer, -0.05, 0.05) # 再次限幅 + env.vehicle.apply_control(control) - # 控制车辆(油门已在环境中固定为0.5) - env.control_vehicle(0.5, steer) + # 每20步打印状态 + if (step + 1) % 20 == 0: + vehicle_loc = env.vehicle.get_transform().location + print( + f"步骤 {step + 1}/500 | 油门={throttle:.2f}, 转向={steer:.2f} | 位置:x={vehicle_loc.x:.1f}, y={vehicle_loc.y:.1f}") - # 每10步打印状态 - if step % 10 == 0: - print(f"🔹 第{step}步:转向={steer:.3f},价值={value.item():.2f}") + time.sleep(0.02) - time.sleep(0.15) # 稍快的步长,行驶更流畅 + print("\n✅ 道路行驶仿真完成!") - print("\n✅ 仿真结束!车辆全程沿道路直线行驶~") except Exception as e: - print(f"❌ 仿真出错:{e}") + print(f"\n❌ 仿真过程中出错: {e}") finally: - env.close() + if env is not None: + env.cleanup() + print("\n🔚 仿真结束,所有资源已清理") if __name__ == "__main__": - print(f"📌 Python版本:{sys.version.split()[0]}") - print(f"📌 PyTorch版本:{torch.__version__}") - print(f"📌 CUDA可用:{torch.cuda.is_available()}") - print("=" * 50) run_simulation() \ No newline at end of file From 43a2a3693250279e07f662c695cbfa75f5391c44 Mon Sep 17 00:00:00 2001 From: LELlierle <2958542913@qq.com> Date: Mon, 22 Dec 2025 21:36:02 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E4=BD=BF=E5=B0=8F=E8=BD=A6=E7=B2=BE?= =?UTF-8?q?=E5=87=86=E9=81=BF=E5=BC=80=E9=9A=9C=E7=A2=8D=E7=89=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mmap_drl_nav/run_simulation.py | 341 +++++++++++++++++++---------- 1 file changed, 225 insertions(+), 116 deletions(-) diff --git a/src/mmap_drl_nav/run_simulation.py b/src/mmap_drl_nav/run_simulation.py index 6ac3a7ef2c..65e75047aa 100644 --- a/src/mmap_drl_nav/run_simulation.py +++ b/src/mmap_drl_nav/run_simulation.py @@ -2,93 +2,164 @@ import time import carla # CARLA官方Python API import numpy as np - - -# 感知模块(保持原逻辑) +from dataclasses import dataclass +from typing import Optional, Tuple, List +import warnings + +# 忽略CARLA API的冗余警告 +warnings.filterwarnings("ignore", category=UserWarning, module="carla") + + +# ===================== 配置常量 ===================== +@dataclass(frozen=True) +class Config: + """全局配置常量,集中管理参数便于修改""" + CARLA_HOST: str = "localhost" + CARLA_PORT: int = 2000 + CARLA_TIMEOUT: float = 15.0 + SIMULATION_STEPS: int = 500 + STEP_SLEEP: float = 0.02 + VEHICLE_MODEL: str = "model3" + SPAWN_POINT_INDEX: int = 20 + WAYPOINT_DISTANCE: float = 8.0 + THROTTLE_MIN: float = 0.2 + THROTTLE_MAX: float = 0.5 + COLLISION_BRAKE_DURATION: float = 0.5 + SPECTATOR_OFFSET: Tuple[float, float, float] = (-5.0, 0.0, 2.0) + + +# ===================== 感知与决策模块 ===================== class PerceptionModule(torch.nn.Module): - def forward(self, imu_data, image, lidar_data): + """感知模块:模拟多传感器数据处理""" + + def forward(self, imu_data: torch.Tensor, image: torch.Tensor, lidar_data: torch.Tensor) -> Tuple[ + torch.Tensor, ...]: + """ + 前向传播:生成模拟的感知输出 + Args: + imu_data: IMU数据 (batch, 6) + image: 图像数据 (batch, 3, H, W) + lidar_data: LiDAR数据 (batch, 1, H, W) + Returns: + scene_info, segmentation, odometry, obstacles, boundary + """ batch_size = image.shape[0] - scene_info = torch.randn(batch_size, 128).to(image.device) - segmentation = torch.randn(batch_size, 64, 256, 256).to(image.device) - odometry = torch.randn(batch_size, 32).to(image.device) - obstacles = torch.randn(batch_size, 64).to(image.device) - boundary = torch.randn(batch_size, 32).to(image.device) + device = image.device + + # 统一使用device参数,避免重复调用image.device + scene_info = torch.randn(batch_size, 128, device=device) + segmentation = torch.randn(batch_size, 64, 256, 256, device=device) + odometry = torch.randn(batch_size, 32, device=device) + obstacles = torch.randn(batch_size, 64, device=device) + boundary = torch.randn(batch_size, 32, device=device) + return scene_info, segmentation, odometry, obstacles, boundary -# 跨域注意力模块(保持原逻辑) class CrossDomainAttention(torch.nn.Module): - def __init__(self, num_blocks=6): + """跨域注意力模块:融合多模态感知特征""" + + def __init__(self, num_blocks: int = 6): super().__init__() self.num_blocks = num_blocks - input_dim = 128 + 64 * 256 * 256 + 32 + 64 + 32 + # 预计算输入维度,提高可读性 + input_dim = 128 + (64 * 256 * 256) + 32 + 64 + 32 self.fc = torch.nn.Linear(input_dim, 256) - def forward(self, scene_info, segmentation, odometry, obstacles, boundary): + def forward(self, scene_info: torch.Tensor, segmentation: torch.Tensor, + odometry: torch.Tensor, obstacles: torch.Tensor, boundary: torch.Tensor) -> torch.Tensor: + """融合多模态特征""" seg_flat = segmentation.flatten(1) all_features = torch.cat([scene_info, seg_flat, odometry, obstacles, boundary], dim=1) fused = self.fc(all_features) return fused -# 决策模块(道路约束,适配0.9.11) class DecisionModule(torch.nn.Module): + """决策模块:基于融合特征输出车辆控制指令""" + def __init__(self): super().__init__() self.steer_fc = torch.nn.Linear(256, 1) # 转向输出 self.throttle_fc = torch.nn.Linear(256, 1) # 油门输出 - def forward(self, fused_features, target_steer): - # 转向:向目标转向角靠拢,范围[-1,1] - steer = torch.nn.functional.tanh(self.steer_fc(fused_features) + target_steer) - # 油门:限制在[0.2, 0.5],避免过快/过慢 - throttle = torch.nn.functional.sigmoid(self.throttle_fc(fused_features)) * 0.3 + 0.2 + def forward(self, fused_features: torch.Tensor, target_steer: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """ + 输出控制策略 + Args: + fused_features: 融合特征 (batch, 256) + target_steer: 目标转向角 (batch, 1) + Returns: + policy: 控制策略 [throttle, steer] (batch, 2) + value: 价值估计 (batch, 1) + """ + # 转向控制:向目标转向角靠拢,范围[-1,1] + steer = torch.tanh(self.steer_fc(fused_features) + target_steer) + # 油门控制:限制在[0.2, 0.5] + throttle = torch.sigmoid(self.throttle_fc(fused_features)) * ( + Config.THROTTLE_MAX - Config.THROTTLE_MIN) + Config.THROTTLE_MIN policy = torch.cat([throttle, steer], dim=1) - value = torch.randn(fused_features.shape[0], 1) + value = torch.randn(fused_features.shape[0], 1, device=fused_features.device) return policy, value -# CARLA环境类(移除get_navigation,适配0.9.11) +# ===================== CARLA环境管理 ===================== class CarlaEnvironment: + """CARLA环境管理器:负责CARLA连接、车辆生成、传感器管理""" + def __init__(self): - self.client = None - self.world = None - self.blueprint_library = None - self.vehicle = None - self.spectator = None - self.collision_sensor = None # 碰撞传感器 - self.collision_occurred = False # 碰撞标记 + self.client: Optional[carla.Client] = None + self.world: Optional[carla.World] = None + self.blueprint_library: Optional[carla.BlueprintLibrary] = None + self.vehicle: Optional[carla.Vehicle] = None + self.spectator: Optional[carla.Actor] = None + self.collision_sensor: Optional[carla.Sensor] = None + self.collision_occurred: bool = False # 碰撞标记 + self._connect_carla() + self._cleanup_actors() # 独立的清理函数 self._spawn_vehicle() - self._init_collision_sensor() # 初始化碰撞检测 + self._init_collision_sensor() self._set_vehicle_view() - def _connect_carla(self): + def _connect_carla(self) -> None: + """连接CARLA服务器""" try: - self.client = carla.Client('localhost', 2000) - self.client.set_timeout(15.0) + self.client = carla.Client(Config.CARLA_HOST, Config.CARLA_PORT) + self.client.set_timeout(Config.CARLA_TIMEOUT) self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() self.spectator = self.world.get_spectator() print("✅ CARLA服务器连接成功!") except Exception as e: raise RuntimeError( - f"❌ 连接CARLA失败!请确认:\n1. CarlaUE4.exe已启动\n2. 端口2000未被占用\n错误详情:{e}" + f"❌ 连接CARLA失败!请确认:\n1. CarlaUE4.exe已启动(版本0.9.11)\n2. 端口{Config.CARLA_PORT}未被占用\n错误详情:{e}" ) - def _spawn_vehicle(self): + def _cleanup_actors(self) -> None: + """清理残留的车辆和传感器,避免资源泄漏""" try: - # 清理残留车辆/传感器 - for actor in self.world.get_actors().filter('*vehicle*'): - actor.destroy() - for actor in self.world.get_actors().filter('*sensor*'): - actor.destroy() + # 按类型批量清理,提高效率 + actor_filters = ["*vehicle*", "*sensor*"] + for filter_str in actor_filters: + for actor in self.world.get_actors().filter(filter_str): + if actor.is_alive: + actor.destroy() + print("✅ 残留Actor清理完成") + except Exception as e: + print(f"⚠️ 清理Actor时警告:{e}") - vehicle_bp = self.blueprint_library.filter('model3')[0] + def _spawn_vehicle(self) -> None: + """生成车辆并初始化状态""" + try: + vehicle_bp = self.blueprint_library.filter(Config.VEHICLE_MODEL)[0] spawn_points = self.world.get_map().get_spawn_points() - # 选开阔直道的生成点(减少初始碰撞) - spawn_point = spawn_points[20] if len(spawn_points) >= 20 else spawn_points[0] + + # 安全选择生成点 + spawn_idx = Config.SPAWN_POINT_INDEX if len(spawn_points) >= Config.SPAWN_POINT_INDEX else 0 + spawn_point = spawn_points[spawn_idx] + self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) # 初始化车辆状态:刹车、空挡 self.vehicle.apply_control(carla.VehicleControl(brake=1.0, gear=1)) @@ -96,135 +167,172 @@ def _spawn_vehicle(self): except Exception as e: raise RuntimeError(f"❌ 车辆生成失败:{e}") - def _init_collision_sensor(self): - """初始化碰撞传感器(0.9.11兼容)""" - collision_bp = self.blueprint_library.find('sensor.other.collision') - self.collision_sensor = self.world.spawn_actor( - collision_bp, carla.Transform(), attach_to=self.vehicle - ) - # 碰撞回调函数:撞障后标记并减速 - self.collision_sensor.listen(lambda event: self._on_collision(event)) - print("✅ 碰撞传感器初始化完成") + def _init_collision_sensor(self) -> None: + """初始化碰撞传感器""" + try: + collision_bp = self.blueprint_library.find('sensor.other.collision') + self.collision_sensor = self.world.spawn_actor( + collision_bp, carla.Transform(), attach_to=self.vehicle + ) + self.collision_sensor.listen(self._on_collision) + print("✅ 碰撞传感器初始化完成") + except Exception as e: + raise RuntimeError(f"❌ 碰撞传感器初始化失败:{e}") - def _on_collision(self, event): - """碰撞发生时的处理""" + def _on_collision(self, event: carla.CollisionEvent) -> None: + """碰撞回调函数:处理碰撞事件""" if not self.collision_occurred: self.collision_occurred = True - print("⚠️ 检测到碰撞!立即减速并调整方向") - # 撞障后先刹车 + print(f"⚠️ 检测到碰撞!碰撞对象:{event.other_actor.type_id}") + # 撞障后紧急刹车 self.vehicle.apply_control(carla.VehicleControl(brake=1.0, throttle=0.0)) - time.sleep(0.5) + time.sleep(Config.COLLISION_BRAKE_DURATION) - def get_target_steer(self): + def get_target_steer(self) -> torch.Tensor: """ - 适配0.9.11的道路转向逻辑: - 1. 用get_waypoint获取当前道路点 - 2. 计算朝向路点的转向角(无navigation接口的替代方案) + 计算目标转向角(适配CARLA 0.9.11) + Returns: + target_steer: 目标转向角 (1, 1) """ if self.collision_occurred: - # 撞障后反向微调,避开障碍物 + # 撞障后反向微调 self.collision_occurred = False - return torch.tensor([[0.3]], dtype=torch.float32) # 小幅向右调 + return torch.tensor([[0.3]], dtype=torch.float32) - # 核心:0.9.11兼容的路点获取方式 + # 获取当前车辆位置和路点 vehicle_location = self.vehicle.get_transform().location - # project_to_road=True:将车辆位置投影到最近的道路上 current_waypoint = self.world.get_map().get_waypoint( vehicle_location, project_to_road=True, lane_type=carla.LaneType.Driving ) - # 获取前方8米的路点(0.9.11支持next()接口) - next_waypoint = current_waypoint.next(8.0)[0] - - # 计算车辆到下一个路点的转向误差 - vehicle_transform = self.vehicle.get_transform() - # 车辆当前前进方向的向量 - vehicle_forward = vehicle_transform.get_forward_vector() - # 车辆到下一个路点的方向向量 + + # 获取前方路点(处理空列表情况) + next_waypoints = current_waypoint.next(Config.WAYPOINT_DISTANCE) + if not next_waypoints: + return torch.tensor([[0.0]], dtype=torch.float32) + next_waypoint = next_waypoints[0] + + # 计算转向误差 + vehicle_forward = self.vehicle.get_transform().get_forward_vector() direction_to_next = next_waypoint.transform.location - vehicle_location - # 归一化向量 + + # 向量归一化(添加防除零保护) vehicle_forward = np.array([vehicle_forward.x, vehicle_forward.y]) direction_to_next = np.array([direction_to_next.x, direction_to_next.y]) - vehicle_forward = vehicle_forward / np.linalg.norm(vehicle_forward) - direction_to_next = direction_to_next / np.linalg.norm(direction_to_next) - # 计算夹角(转向误差),归一化到[-1,1] + norm_forward = np.linalg.norm(vehicle_forward) + norm_next = np.linalg.norm(direction_to_next) + + if norm_forward < 1e-6 or norm_next < 1e-6: + return torch.tensor([[0.0]], dtype=torch.float32) + + vehicle_forward = vehicle_forward / norm_forward + direction_to_next = direction_to_next / norm_next + + # 计算夹角并归一化到[-1,1] dot_product = np.dot(vehicle_forward, direction_to_next) cross_product = np.cross(vehicle_forward, direction_to_next) - steer_error = np.arcsin(cross_product) / np.pi # 弧度转[-0.5,0.5],再放大到[-1,1] + steer_error = np.arcsin(cross_product) / np.pi # 弧度转[-0.5,0.5] steer_error = np.clip(steer_error * 2, -1.0, 1.0) return torch.tensor([[steer_error]], dtype=torch.float32) - def _set_vehicle_view(self): - if self.vehicle and self.spectator: - transform = self.vehicle.get_transform() - spectator_transform = carla.Transform( - transform.location + carla.Location(x=-5, z=2), - transform.rotation - ) - self.spectator.set_transform(spectator_transform) - print("✅ 视角已切换到车辆后方!") - print(" 🎮 WASD:移动视角 | 鼠标右键+拖动:旋转视角 | 滚轮:缩放 | P:快速定位到车辆") + def _set_vehicle_view(self) -> None: + """设置观众视角到车辆后方""" + if not (self.vehicle and self.spectator): + return + + transform = self.vehicle.get_transform() + spectator_transform = carla.Transform( + transform.location + carla.Location(*Config.SPECTATOR_OFFSET), + transform.rotation + ) + self.spectator.set_transform(spectator_transform) + print("✅ 视角已切换到车辆后方!") + print(" 🎮 WASD:移动视角 | 鼠标右键+拖动:旋转视角 | 滚轮:缩放 | P:快速定位到车辆") - def cleanup(self): + def cleanup(self) -> None: + """清理所有资源""" try: - if self.collision_sensor: - self.collision_sensor.destroy() + # 先停止车辆 if self.vehicle and self.vehicle.is_alive: self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=1.0)) time.sleep(0.5) + + # 销毁传感器和车辆 + if self.collision_sensor and self.collision_sensor.is_alive: + self.collision_sensor.destroy() + if self.vehicle and self.vehicle.is_alive: self.vehicle.destroy() + print("✅ 资源已清理") except Exception as e: print(f"⚠️ 清理资源时警告:{e}") -# 集成系统(适配新的决策模块) +# ===================== 集成系统 ===================== class IntegratedSystem: - def __init__(self, device='cpu'): + """集成系统:感知-融合-决策全流程""" + + def __init__(self, device: str = 'cpu'): self.device = device - self.perception = PerceptionModule().to(self.device) - self.attention = CrossDomainAttention(num_blocks=6).to(self.device) - self.decision = DecisionModule().to(self.device) + self.perception = PerceptionModule().to(device) + self.attention = CrossDomainAttention(num_blocks=6).to(device) + self.decision = DecisionModule().to(device) - def forward(self, image, lidar_data, imu_data, target_steer): - """新增target_steer参数,传递道路约束""" + def forward(self, image: torch.Tensor, lidar_data: torch.Tensor, + imu_data: torch.Tensor, target_steer: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """ + 前向推理全流程 + Args: + image: 图像数据 (1, 3, 256, 256) + lidar_data: LiDAR数据 (1, 1, 256, 256) + imu_data: IMU数据 (1, 6) + target_steer: 目标转向角 (1, 1) + Returns: + policy: 控制策略 [throttle, steer] (1, 2) + value: 价值估计 (1, 1) + """ + # 感知处理 scene_info, segmentation, odometry, obstacles, boundary = self.perception(imu_data, image, lidar_data) + # 特征融合 fused_features = self.attention(scene_info, segmentation, odometry, obstacles, boundary) + # 决策输出 policy, value = self.decision(fused_features, target_steer.to(self.device)) return policy, value -# 主仿真函数(道路行驶逻辑) -def run_simulation(): +# ===================== 主函数 ===================== +def run_simulation() -> None: + """运行CARLA自动驾驶仿真""" env = None try: - print("📢 运行前请确认:CarlaUE4.exe已启动(版本0.9.11)") + print(f"📢 运行前请确认:CarlaUE4.exe已启动(版本0.9.11),端口{Config.CARLA_PORT}未被占用") time.sleep(2) + # 初始化环境和系统 env = CarlaEnvironment() device = 'cuda' if torch.cuda.is_available() else 'cpu' print(f"✅ 使用计算设备: {device}") system = IntegratedSystem(device=device) - # 延长仿真步数到500步(约10秒) - print("\n🚗 开始沿道路行驶仿真,共500步...") - for step in range(500): + # 运行仿真 + print(f"\n🚗 开始沿道路行驶仿真,共{Config.SIMULATION_STEPS}步...") + for step in range(Config.SIMULATION_STEPS): # 模拟传感器输入 - image = torch.randn(1, 3, 256, 256).to(device) - lidar_data = torch.randn(1, 1, 256, 256).to(device) - imu_data = torch.randn(1, 6).to(device) + image = torch.randn(1, 3, 256, 256, device=device) + lidar_data = torch.randn(1, 1, 256, 256, device=device) + imu_data = torch.randn(1, 6, device=device) - # 获取道路约束的目标转向角(0.9.11兼容) + # 获取目标转向角 target_steer = env.get_target_steer() - # 前向推理(传递目标转向角) - policy, value = system.forward(image, lidar_data, imu_data, target_steer) + # 前向推理 + policy, _ = system.forward(image, lidar_data, imu_data, target_steer) + + # 解析并应用控制指令 + throttle = float(policy[0][0].cpu().item()) # 移到CPU避免设备不匹配 + steer = float(policy[0][1].cpu().item()) - # 解析策略并应用(限制范围) - throttle = float(policy[0][0]) - steer = float(policy[0][1]) - # 最终控制:碰撞时刹车,否则正常行驶 if env.collision_occurred: control = carla.VehicleControl(throttle=0.0, steer=steer, brake=0.5) else: @@ -232,18 +340,19 @@ def run_simulation(): env.vehicle.apply_control(control) - # 每20步打印状态 + # 定期打印状态 if (step + 1) % 20 == 0: vehicle_loc = env.vehicle.get_transform().location print( - f"步骤 {step + 1}/500 | 油门={throttle:.2f}, 转向={steer:.2f} | 位置:x={vehicle_loc.x:.1f}, y={vehicle_loc.y:.1f}") + f"步骤 {step + 1}/{Config.SIMULATION_STEPS} | 油门={throttle:.2f}, 转向={steer:.2f} | 位置:x={vehicle_loc.x:.1f}, y={vehicle_loc.y:.1f}") - time.sleep(0.02) + time.sleep(Config.STEP_SLEEP) print("\n✅ 道路行驶仿真完成!") except Exception as e: print(f"\n❌ 仿真过程中出错: {e}") + raise # 重新抛出异常便于调试 finally: if env is not None: env.cleanup()