From e36324557d6d037d499e5deb69f8833b7083601f Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 22 Sep 2025 19:24:18 +0800 Subject: [PATCH 01/18] =?UTF-8?q?=E5=A4=9A=E6=A8=A1=E6=80=81=E6=9C=BA?= =?UTF-8?q?=E5=99=A8=E4=BA=BA=E5=AF=BC=E8=88=AA=E7=B3=BB=E7=BB=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mmap_apl/README.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/mmap_apl/README.md diff --git a/src/mmap_apl/README.md b/src/mmap_apl/README.md new file mode 100644 index 0000000000..e69de29bb2 From 09d2901bee83145b794a64ccb180d75766db7bea Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 24 Nov 2025 10:41:18 +0800 Subject: [PATCH 02/18] updata --- src/mmap_apl/README.md | 59 ++++++++++++++++++++++++++++++++++ src/mmap_apl/m.py | 59 ++++++++++++++++++++++++++++++++++ src/mmap_apl/run_simulation.py | 57 ++++++++++++++++++++++++++++++++ 3 files changed, 175 insertions(+) create mode 100644 src/mmap_apl/m.py create mode 100644 src/mmap_apl/run_simulation.py diff --git a/src/mmap_apl/README.md b/src/mmap_apl/README.md index e69de29bb2..096a1089aa 100644 --- a/src/mmap_apl/README.md +++ b/src/mmap_apl/README.md @@ -0,0 +1,59 @@ +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 + +class IntegratedSystem: + def __init__(self, device='cpu', state_dim=128, action_dim=2): + 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.sagm = SelfAssessmentGradientModel(input_dim=state_dim + action_dim).to(self.device) + + def forward(self, image, lidar_data, imu_data, action): + 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) + 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 + 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() + 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))) + loss.backward() + optimizer.step() + running_loss += loss.item() + 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') + +def test_model(model, dataloader, device): + model.eval() + 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) + 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() + avg_loss = total_loss / len(dataloader) + print(f'Test Average Loss: {avg_loss:.4f}' \ No newline at end of file diff --git a/src/mmap_apl/m.py b/src/mmap_apl/m.py new file mode 100644 index 0000000000..096a1089aa --- /dev/null +++ b/src/mmap_apl/m.py @@ -0,0 +1,59 @@ +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 + +class IntegratedSystem: + def __init__(self, device='cpu', state_dim=128, action_dim=2): + 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.sagm = SelfAssessmentGradientModel(input_dim=state_dim + action_dim).to(self.device) + + def forward(self, image, lidar_data, imu_data, action): + 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) + 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 + 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() + 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))) + loss.backward() + optimizer.step() + running_loss += loss.item() + 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') + +def test_model(model, dataloader, device): + model.eval() + 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) + 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() + avg_loss = total_loss / len(dataloader) + print(f'Test Average Loss: {avg_loss:.4f}' \ No newline at end of file diff --git a/src/mmap_apl/run_simulation.py b/src/mmap_apl/run_simulation.py new file mode 100644 index 0000000000..e463886064 --- /dev/null +++ b/src/mmap_apl/run_simulation.py @@ -0,0 +1,57 @@ +import torch +import time # 补充time模块导入，用于sleep延迟 +from models.perception_module import PerceptionModule +from models.attention_module import CrossDomainAttention +from models.decision_module import DecisionModule +from models.dqn_agent import DQNAgent +from envs.carla_environment import CarlaEnvironment # 类名是CarlaEnvironment +import carla + +class IntegratedSystem: + def __init__(self, device='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) + + def forward(self, image, lidar_data, imu_data): + # 注意：确保perception的输入参数顺序与定义一致 + 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) + return policy, value + +def run_simulation(): + # 实例化CARLA环境（使用正确的类名） + env = CarlaEnvironment() + # 确保环境初始化成功（例如连接CARLA服务器、生成车辆等） + # 建议添加检查：if not env.initialized: raise Exception("CARLA环境初始化失败") + + # 初始化集成系统 + system = IntegratedSystem(device='cuda' if torch.cuda.is_available() else 'cpu') + + try: # 使用try-finally确保环境正确关闭 + for _ in range(100): # 运行100个模拟步骤 + # 注意：实际中应从env获取真实数据，而非随机生成 + image = torch.randn(3, 256, 256).unsqueeze(0).to(system.device) + lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(system.device) + imu_data = torch.randn(1, 6).to(system.device) + + # 推理得到策略 + policy, value = system.forward(image, lidar_data, imu_data) + + # 将策略转换为CARLA控制信号（确保policy的维度正确） + # 假设policy[0][0]是油门，policy[0][1]是转向角 + control = carla.VehicleControl( + throttle=float(policy[0][0].clamp(0, 1)), # 油门范围[0,1] + steer=float(policy[0][1].clamp(-1, 1)) # 转向角范围[-1,1] + ) + env.vehicle.apply_control(control) # 应用控制信号 + + time.sleep(0.1) # 模拟物理时间步长 + finally: + # 确保仿真结束后清理环境（关闭连接、销毁 Actors等） + env.cleanup() # 假设CarlaEnvironment类有cleanup方法 + +if __name__ == "__main__": + run_simulation() From de6d012951ad09a127bc7f05f92b56b75698b560 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 24 Nov 2025 11:25:43 +0800 Subject: [PATCH 03/18] updata --- src/mmap_apl/m.py | 59 ----------------------------------------------- 1 file changed, 59 deletions(-) delete mode 100644 src/mmap_apl/m.py diff --git a/src/mmap_apl/m.py b/src/mmap_apl/m.py deleted file mode 100644 index 096a1089aa..0000000000 --- a/src/mmap_apl/m.py +++ /dev/null @@ -1,59 +0,0 @@ -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 - -class IntegratedSystem: - def __init__(self, device='cpu', state_dim=128, action_dim=2): - 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.sagm = SelfAssessmentGradientModel(input_dim=state_dim + action_dim).to(self.device) - - def forward(self, image, lidar_data, imu_data, action): - 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) - 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 - 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() - 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))) - loss.backward() - optimizer.step() - running_loss += loss.item() - 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') - -def test_model(model, dataloader, device): - model.eval() - 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) - 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() - avg_loss = total_loss / len(dataloader) - print(f'Test Average Loss: {avg_loss:.4f}' \ No newline at end of file From 5d0d55806d8ac3ad2495d5b3136e5cf4b5c85699 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 24 Nov 2025 18:50:22 +0800 Subject: [PATCH 04/18] updata --- src/{mmap_apl => MMAP-DRL-Nav}/README.md | 0 src/{mmap_apl => MMAP-DRL-Nav}/run_simulation.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{mmap_apl => MMAP-DRL-Nav}/README.md (100%) rename src/{mmap_apl => MMAP-DRL-Nav}/run_simulation.py (100%) diff --git a/src/mmap_apl/README.md b/src/MMAP-DRL-Nav/README.md similarity index 100% rename from src/mmap_apl/README.md rename to src/MMAP-DRL-Nav/README.md diff --git a/src/mmap_apl/run_simulation.py b/src/MMAP-DRL-Nav/run_simulation.py similarity index 100% rename from src/mmap_apl/run_simulation.py rename to src/MMAP-DRL-Nav/run_simulation.py From d7120f4f898a5f8ca3eb21677f654c20337e9eda Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 24 Nov 2025 22:37:43 +0800 Subject: [PATCH 05/18] updata' --- src/MMAP-DRL-Nav/README.md | 59 ------------------------------ src/MMAP-DRL-Nav/run_simulation.py | 58 ++++++++++++++--------------- 2 files changed, 27 insertions(+), 90 deletions(-) delete mode 100644 src/MMAP-DRL-Nav/README.md diff --git a/src/MMAP-DRL-Nav/README.md b/src/MMAP-DRL-Nav/README.md deleted file mode 100644 index 096a1089aa..0000000000 --- a/src/MMAP-DRL-Nav/README.md +++ /dev/null @@ -1,59 +0,0 @@ -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 - -class IntegratedSystem: - def __init__(self, device='cpu', state_dim=128, action_dim=2): - 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.sagm = SelfAssessmentGradientModel(input_dim=state_dim + action_dim).to(self.device) - - def forward(self, image, lidar_data, imu_data, action): - 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) - 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 - 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() - 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))) - loss.backward() - optimizer.step() - running_loss += loss.item() - 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') - -def test_model(model, dataloader, device): - model.eval() - 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) - 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() - avg_loss = total_loss / len(dataloader) - print(f'Test Average Loss: {avg_loss:.4f}' \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/run_simulation.py b/src/MMAP-DRL-Nav/run_simulation.py index e463886064..3f72c6fb5b 100644 --- a/src/MMAP-DRL-Nav/run_simulation.py +++ b/src/MMAP-DRL-Nav/run_simulation.py @@ -1,57 +1,53 @@ +# 1. 导入模块（放在最开头） import torch -import time # 补充time模块导入，用于sleep延迟 +import time from models.perception_module import PerceptionModule from models.attention_module import CrossDomainAttention from models.decision_module import DecisionModule from models.dqn_agent import DQNAgent -from envs.carla_environment import CarlaEnvironment # 类名是CarlaEnvironment +from envs.carla_environment import CarlaEnvironment import carla +# 2. 定义 IntegratedSystem 类 class IntegratedSystem: def __init__(self, device='cpu'): self.device = device self.perception = PerceptionModule().to(self.device) - self.attention = CrossDomainAttention(num_blocks=6).to(self.device) + # 补充 input_dims 参数（与感知模块输出维度匹配） + self.attention = CrossDomainAttention( + num_blocks=6, + input_dims=[256, 256, 6, 256, 256] + ).to(self.device) self.decision = DecisionModule().to(self.device) def forward(self, image, lidar_data, imu_data): - # 注意：确保perception的输入参数顺序与定义一致 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) return policy, value +# 3. 定义 run_simulation 函数 def run_simulation(): - # 实例化CARLA环境（使用正确的类名） - env = CarlaEnvironment() - # 确保环境初始化成功（例如连接CARLA服务器、生成车辆等） - # 建议添加检查：if not env.initialized: raise Exception("CARLA环境初始化失败") - - # 初始化集成系统 + env = CarlaEnvironment() # 初始化CARLA环境 system = IntegratedSystem(device='cuda' if torch.cuda.is_available() else 'cpu') - try: # 使用try-finally确保环境正确关闭 - for _ in range(100): # 运行100个模拟步骤 - # 注意：实际中应从env获取真实数据，而非随机生成 - image = torch.randn(3, 256, 256).unsqueeze(0).to(system.device) - lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(system.device) - imu_data = torch.randn(1, 6).to(system.device) + for _ in range(100): # 运行100步仿真 + # 生成随机传感器数据（实际中应从env获取真实数据） + image = torch.randn(3, 256, 256).unsqueeze(0).to(system.device) + lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(system.device) + imu_data = torch.randn(1, 6).to(system.device) - # 推理得到策略 - policy, value = system.forward(image, lidar_data, imu_data) - - # 将策略转换为CARLA控制信号（确保policy的维度正确） - # 假设policy[0][0]是油门，policy[0][1]是转向角 - control = carla.VehicleControl( - throttle=float(policy[0][0].clamp(0, 1)), # 油门范围[0,1] - steer=float(policy[0][1].clamp(-1, 1)) # 转向角范围[-1,1] - ) - env.vehicle.apply_control(control) # 应用控制信号 + # 前向传播得到策略 + policy, value = system.forward(image, lidar_data, imu_data) + + # 转换为CARLA控制信号（限制范围避免异常） + throttle = float(torch.clamp(policy[0][0], 0, 1)) # 油门范围[0,1] + steer = float(torch.clamp(policy[0][1], -1, 1)) # 转向范围[-1,1] + control = carla.VehicleControl(throttle=throttle, steer=steer) + env.vehicle.apply_control(control) - time.sleep(0.1) # 模拟物理时间步长 - finally: - # 确保仿真结束后清理环境（关闭连接、销毁 Actors等） - env.cleanup() # 假设CarlaEnvironment类有cleanup方法 + time.sleep(0.1) # 模拟时间间隔 +# 4. 程序入口（放在最后） if __name__ == "__main__": - run_simulation() + run_simulation() \ No newline at end of file From 4714bddf4986bd69690ad674f4abe80516981e06 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Tue, 25 Nov 2025 15:39:52 +0800 Subject: [PATCH 06/18] updata --- src/MMAP-DRL-Nav/requirements.txt | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 src/MMAP-DRL-Nav/requirements.txt diff --git a/src/MMAP-DRL-Nav/requirements.txt b/src/MMAP-DRL-Nav/requirements.txt new file mode 100644 index 0000000000..f2ffb1e2ae --- /dev/null +++ b/src/MMAP-DRL-Nav/requirements.txt @@ -0,0 +1,8 @@ +torch==1.7.1 +torchvision==0.8.2 +carla==0.9.11 +numpy==1.19.5 +opencv-python==4.5.1.48 +Pillow==8.1.0 +pygame==2.0.1 +scipy==1.6.0 From d92eb2473175b9a7b5fabeb58ac4dbc7f0dc4ad5 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Tue, 25 Nov 2025 22:06:30 +0800 Subject: [PATCH 07/18] updata --- src/MMAP-DRL-Nav/attention_module.py | 48 ++++++++++++ src/MMAP-DRL-Nav/carla_environment.py | 102 ++++++++++++++++++++++++++ src/MMAP-DRL-Nav/decision_module.py | 25 +++++++ src/MMAP-DRL-Nav/dqn_agent.py | 87 ++++++++++++++++++++++ 4 files changed, 262 insertions(+) create mode 100644 src/MMAP-DRL-Nav/attention_module.py create mode 100644 src/MMAP-DRL-Nav/carla_environment.py create mode 100644 src/MMAP-DRL-Nav/decision_module.py create mode 100644 src/MMAP-DRL-Nav/dqn_agent.py diff --git a/src/MMAP-DRL-Nav/attention_module.py b/src/MMAP-DRL-Nav/attention_module.py new file mode 100644 index 0000000000..8a80987e4c --- /dev/null +++ b/src/MMAP-DRL-Nav/attention_module.py @@ -0,0 +1,48 @@ +import torch +import torch.nn as nn + +class CrossDomainAttention(nn.Module): + def __init__(self, input_dims, num_blocks=6): + super(CrossDomainAttention, self).__init__() + # 多头注意力模块（embed_dim 需与投影后的维度一致，即256） + self.attention_blocks = nn.ModuleList([ + nn.MultiheadAttention(embed_dim=256, num_heads=8) for _ in range(num_blocks) + ]) + # 投影层：将每个输入特征映射到256维 + self.proj_layers = nn.ModuleList([ + nn.Linear(dim, 256) for dim in input_dims + ]) + # 可选：层归一化，稳定训练 + self.norm_layers = nn.ModuleList([ + nn.LayerNorm(256) for _ in range(num_blocks) + ]) + + def forward(self, *inputs): + processed_inputs = [] + for i, x in enumerate(inputs): + # 1. 处理高维特征（如 segmentation 的 (B, 256, H, W)） + if x.dim() > 2: + # 对空间维度求平均，降为 (B, C)，其中 C=256（与 input_dims 对应） + x = x.mean(dim=tuple(range(2, x.dim()))) # 保留前2维 (B, C) + + # 2. 确保输入线性层的是2维张量 (B, D) + assert x.dim() == 2, f"特征 {i} 经处理后必须是2维张量，当前维度：{x.dim()}" + + # 3. 投影到256维，并调整为 MultiheadAttention 要求的格式 (seq_len=1, B, 256) + # （因为每个特征是全局描述，序列长度设为1） + x = self.proj_layers[i](x) # (B, 256) + x = x.unsqueeze(0) # (1, B, 256)，seq_len=1 + processed_inputs.append(x) + + # 4. 拼接所有跨域特征，形成序列 (seq_len=N, B, 256)，其中 N=5（5个特征） + x = torch.cat(processed_inputs, dim=0) # (5, B, 256) + + # 5. 经过多个注意力块 + for i, block in enumerate(self.attention_blocks): + # 自注意力计算 + attn_output, _ = block(x, x, x) # (5, B, 256) + # 残差连接 + 层归一化 + x = self.norm_layers[i](x + attn_output) + + # 6. 对序列维度求平均，得到融合后的特征 (B, 256) + return x.mean(dim=0) # (B, 256) \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py new file mode 100644 index 0000000000..abbebba7ee --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -0,0 +1,102 @@ +import carla +import numpy as np +import gym + +class CarlaEnvironment(gym.Env): + def __init__(self): + super(CarlaEnvironment, self).__init__() + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) # 增加超时时间，避免连接失败 + self.world = self.client.get_world() + self.blueprint_library = self.world.get_blueprint_library() # 缓存蓝图库 + + # 动作和观测空间保持不变 + self.action_space = gym.spaces.Discrete(4) + self.observation_space = gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8) + + self.vehicle = None + self.camera = None # 后续可添加相机传感器 + + # 初始化时直接生成车辆（或在首次reset时生成） + self.reset() # 确保车辆在环境创建时就初始化 + + def reset(self): + # 清理现有车辆 + if self.vehicle is not None: + self.vehicle.destroy() + self.vehicle = None + + # 选择车辆蓝图（优先选可驾驶的车辆） + vehicle_bp = self.blueprint_library.filter('vehicle.*')[0] + vehicle_bp.set_attribute('role_name', 'hero') # 标记为主车辆 + + # 选择有效的spawn点（优先用地图默认点） + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + # 如果没有默认点，使用自定义点（但尽量避免） + spawn_point = carla.Transform(carla.Location(x=20, y=0, z=0.5)) # 调整到合理位置 + else: + spawn_point = spawn_points[0] # 用第一个默认点 + + # 生成车辆并检查是否成功 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if self.vehicle is None: + # 尝试另一个spawn点（防止第一个点被占用） + if len(spawn_points) > 1: + spawn_point = spawn_points[1] + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if self.vehicle is None: + raise RuntimeError("无法生成车辆，请检查spawn点是否有效或CARLA是否正常运行") + + # 禁用自动驾驶，由代码控制 + self.vehicle.set_autopilot(False) + self.world.tick() + + return self.get_observation() + + def get_observation(self): + # 实际项目中需要添加相机传感器获取图像，这里先返回模拟数据 + # 示例：添加相机（简化版） + if self.camera is None: + camera_bp = self.blueprint_library.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '128') + camera_bp.set_attribute('image_size_y', '128') + # 相机安装在车辆前方 + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) + self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) + # 这里需要设置相机回调函数获取图像，简化起见返回随机图像 + return np.random.randint(0, 256, size=(128, 128, 3), dtype=np.uint8) + + def step(self, action): + if self.vehicle is None: + raise RuntimeError("车辆未初始化，请先调用reset()") + + # 控制信号限制在有效范围（避免极端值） + throttle = 0.0 + steer = 0.0 + if action == 0: # 前进 + throttle = 0.5 # 适中油门，避免速度过快 + elif action == 1: # 左转 + throttle = 0.3 + steer = -0.5 + elif action == 2: # 右转 + throttle = 0.3 + steer = 0.5 + elif action == 3: # 后退 + throttle = -0.3 # 后退油门绝对值较小 + + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer)) + self.world.tick() + + next_state = self.get_observation() + reward = 1.0 # 后续需根据任务设计奖励函数 + done = False + return next_state, reward, done, {} + + def close(self): + # 清理所有生成的actor（车辆、相机等） + if self.camera is not None: + self.camera.destroy() + if self.vehicle is not None: + self.vehicle.destroy() + print("环境已清理") \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/decision_module.py b/src/MMAP-DRL-Nav/decision_module.py new file mode 100644 index 0000000000..7695d2d4aa --- /dev/null +++ b/src/MMAP-DRL-Nav/decision_module.py @@ -0,0 +1,25 @@ +import torch +import torch.nn as nn + +class DecisionModule(nn.Module): + def __init__(self): + super(DecisionModule, self).__init__() + self.policy_net = nn.Sequential( + nn.Linear(256, 128), + nn.ReLU(), + nn.Linear(128, 64), + nn.ReLU(), + nn.Linear(64, 2) # 输出：转向角和油门 + ) + self.value_net = nn.Sequential( + nn.Linear(256, 128), + nn.ReLU(), + nn.Linear(128, 64), + nn.ReLU(), + nn.Linear(64, 1) # 值函数 + ) + + def forward(self, fused_features): + policy = self.policy_net(fused_features) + value = self.value_net(fused_features) + return policy, value diff --git a/src/MMAP-DRL-Nav/dqn_agent.py b/src/MMAP-DRL-Nav/dqn_agent.py new file mode 100644 index 0000000000..2ef914b102 --- /dev/null +++ b/src/MMAP-DRL-Nav/dqn_agent.py @@ -0,0 +1,87 @@ +import torch +import torch.nn as nn +import numpy as np +from .pruning import prune_model +from .quantization import quantize_model +class DQNAgent: + def __init__(self, state_size, action_size, config): + self.state_size = state_size + self.action_size = action_size + self.memory = [] + self.gamma = 0.95 # 折扣因子 + self.epsilon = 1.0 # 探索率 + self.epsilon_decay = config['agent']['epsilon_decay'] + self.epsilon_min = config['agent']['epsilon_min'] + self.learning_rate = config['train']['learning_rate'] + self.model = self._build_model() + + def _build_model(self): + model = nn.Sequential( + nn.Linear(self.state_size, 24), + nn.ReLU(), + nn.Linear(24, 24), + nn.ReLU(), + nn.Linear(24, self.action_size) + ) + return model + + def remember(self, state, action, reward, next_state, done): + self.memory.append((state, action, reward, next_state, done)) + + def act(self, state): + if np.random.rand() <= self.epsilon: + return np.random.choice(self.action_size) + q_values = self.model(torch.FloatTensor(state)) + return np.argmax(q_values.detach().numpy()) + + def replay(self, batch_size): + minibatch = np.random.choice(self.memory, batch_size) + for state, action, reward, next_state, done in minibatch: + target = reward + if not done: + target += self.gamma * np.amax(self.model(torch.FloatTensor(next_state)).detach().numpy()) + target_f = self.model(torch.FloatTensor(state)) + target_f[action] = target + self.model.fit(torch.FloatTensor(state), target_f.unsqueeze(0), epochs=1, verbose=0) + if self.epsilon > self.epsilon_min: + self.epsilon *= self.epsilon_decay + + def calculate_reward(self, current_position, target_position, road_position, done): + #计算奖励 + #:param current_position: 当前机器人位置 (x, y) + #:param target_position: 目标位置 (x, y) + #:param road_position: 机器人在道路上的位置 + #:param done: 是否完成任务 + #:return: 奖励值 + distance_to_target = np.linalg.norm(current_position - target_position) + distance_to_road = np.linalg.norm(current_position - road_position) + + if done: + return 100 # 到达目标的奖励 + elif distance_to_target < 1.0: # 靠近目标位置 + return 10 # 靠近目标的奖励 + elif distance_to_road > 1.0: # 远离道路 + return -5 # 远离道路的惩罚 + elif distance_to_target < 5.0: # 在某个距离范围内 + return 1 # 轻微奖励 + else: + return -1 # 远离目标的惩罚 + + def get_state(self, position, orientation, target_position, road_position): + + #获取状态 + #:param position: 机器人当前位置 (x, y) + #:param orientation: 机器人朝向 (angle) + #:param target_position: 目标位置 (x, y) + #:param road_position: 机器人在道路上的位置 + # :return: 状态数组 + state = np.array([ + position[0], # 当前 x 坐标 + position[1], # 当前 y 坐标 + orientation, # 当前朝向 + target_position[0], # 目标 x 坐标 + target_position[1], # 目标 y 坐标 + road_position[0], # 道路 x 坐标 + road_position[1] # 道路 y 坐标 + ]) + return state From 549657dc31cb00aea62c69a97081b8bd5fad7d6e Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 1 Dec 2025 09:49:28 +0800 Subject: [PATCH 08/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 70 +++++++++++++++------------ 1 file changed, 40 insertions(+), 30 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index abbebba7ee..0fe2383c90 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -6,95 +6,105 @@ class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) # 增加超时时间，避免连接失败 + self.client.set_timeout(10.0) self.world = self.client.get_world() - self.blueprint_library = self.world.get_blueprint_library() # 缓存蓝图库 + self.blueprint_library = self.world.get_blueprint_library() - # 动作和观测空间保持不变 self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8) self.vehicle = None - self.camera = None # 后续可添加相机传感器 + self.camera = None - # 初始化时直接生成车辆（或在首次reset时生成） - self.reset() # 确保车辆在环境创建时就初始化 + # 新增：镜头跟随参数（仅用于初始化跳转） + self.spectator_offset = carla.Location(x=0, y=0, z=2.5) + self.spectator_distance = -5.0 + self.spectator_pitch = -10 + + self.reset() def reset(self): - # 清理现有车辆 if self.vehicle is not None: self.vehicle.destroy() self.vehicle = None - # 选择车辆蓝图（优先选可驾驶的车辆） vehicle_bp = self.blueprint_library.filter('vehicle.*')[0] - vehicle_bp.set_attribute('role_name', 'hero') # 标记为主车辆 + vehicle_bp.set_attribute('role_name', 'hero') - # 选择有效的spawn点（优先用地图默认点） spawn_points = self.world.get_map().get_spawn_points() if not spawn_points: - # 如果没有默认点，使用自定义点（但尽量避免） - spawn_point = carla.Transform(carla.Location(x=20, y=0, z=0.5)) # 调整到合理位置 + spawn_point = carla.Transform(carla.Location(x=20, y=0, z=0.5)) else: - spawn_point = spawn_points[0] # 用第一个默认点 + spawn_point = spawn_points[0] - # 生成车辆并检查是否成功 self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if self.vehicle is None: - # 尝试另一个spawn点（防止第一个点被占用） if len(spawn_points) > 1: spawn_point = spawn_points[1] self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if self.vehicle is None: - raise RuntimeError("无法生成车辆，请检查spawn点是否有效或CARLA是否正常运行") + raise RuntimeError("无法生成车辆") - # 禁用自动驾驶，由代码控制 self.vehicle.set_autopilot(False) self.world.tick() + + # 仅保留：车辆生成时，镜头跳转到车辆旁（核心需求） + self.follow_vehicle() return self.get_observation() + # 镜头跳转核心方法（仅初始化时调用一次） + def follow_vehicle(self): + spectator = self.world.get_spectator() + if not spectator or not self.vehicle: + return + vehicle_transform = self.vehicle.get_transform() + camera_location = vehicle_transform.location + carla.Location(x=self.spectator_distance) + self.spectator_offset + camera_rotation = carla.Rotation( + pitch=self.spectator_pitch, + yaw=vehicle_transform.rotation.yaw, + roll=0 + ) + spectator.set_transform(carla.Transform(camera_location, camera_rotation)) + def get_observation(self): - # 实际项目中需要添加相机传感器获取图像，这里先返回模拟数据 - # 示例：添加相机（简化版） if self.camera is None: camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') - # 相机安装在车辆前方 camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - # 这里需要设置相机回调函数获取图像，简化起见返回随机图像 return np.random.randint(0, 256, size=(128, 128, 3), dtype=np.uint8) def step(self, action): if self.vehicle is None: raise RuntimeError("车辆未初始化，请先调用reset()") - # 控制信号限制在有效范围（避免极端值） throttle = 0.0 steer = 0.0 - if action == 0: # 前进 - throttle = 0.5 # 适中油门，避免速度过快 - elif action == 1: # 左转 + if action == 0: + throttle = 0.5 + elif action == 1: throttle = 0.3 steer = -0.5 - elif action == 2: # 右转 + elif action == 2: throttle = 0.3 steer = 0.5 - elif action == 3: # 后退 - throttle = -0.3 # 后退油门绝对值较小 + elif action == 3: + throttle = -0.3 self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer)) self.world.tick() + + # 已删除：step里的镜头跟随逻辑 → 后续车辆移动，镜头不再更新 + # self.follow_vehicle() # 这行已删掉 next_state = self.get_observation() - reward = 1.0 # 后续需根据任务设计奖励函数 + reward = 1.0 done = False return next_state, reward, done, {} def close(self): - # 清理所有生成的actor（车辆、相机等） if self.camera is not None: self.camera.destroy() if self.vehicle is not None: From aab293952bf1d468504d83f677b193c2ba9b5f10 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Tue, 2 Dec 2025 09:03:37 +0800 Subject: [PATCH 09/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 93 ++++----------------------- 1 file changed, 11 insertions(+), 82 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index 6166a09f24..cf898d9d0a 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -6,23 +6,17 @@ class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() self.client = carla.Client('localhost', 2000) -<<<<<<< HEAD + self.client.set_timeout(10.0) self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() -======= - self.client.set_timeout(10.0) # 增加超时时间，避免连接失败 - self.world = self.client.get_world() - self.blueprint_library = self.world.get_blueprint_library() # 缓存蓝图库 - - # 动作和观测空间保持不变 ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 + self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8) self.vehicle = None -<<<<<<< HEAD + self.camera = None # 新增：镜头跟随参数（仅用于初始化跳转） @@ -33,20 +27,12 @@ def __init__(self): self.reset() def reset(self): -======= - self.camera = None # 后续可添加相机传感器 - - # 初始化时直接生成车辆（或在首次reset时生成） - self.reset() # 确保车辆在环境创建时就初始化 - def reset(self): - # 清理现有车辆 ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 if self.vehicle is not None: self.vehicle.destroy() self.vehicle = None -<<<<<<< HEAD + vehicle_bp = self.blueprint_library.filter('vehicle.*')[0] vehicle_bp.set_attribute('role_name', 'hero') @@ -58,29 +44,12 @@ def reset(self): self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if self.vehicle is None: -======= - # 选择车辆蓝图（优先选可驾驶的车辆） - vehicle_bp = self.blueprint_library.filter('vehicle.*')[0] - vehicle_bp.set_attribute('role_name', 'hero') # 标记为主车辆 - - # 选择有效的spawn点（优先用地图默认点） - spawn_points = self.world.get_map().get_spawn_points() - if not spawn_points: - # 如果没有默认点，使用自定义点（但尽量避免） - spawn_point = carla.Transform(carla.Location(x=20, y=0, z=0.5)) # 调整到合理位置 - else: - spawn_point = spawn_points[0] # 用第一个默认点 - - # 生成车辆并检查是否成功 - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) - if self.vehicle is None: - # 尝试另一个spawn点（防止第一个点被占用） ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 + if len(spawn_points) > 1: spawn_point = spawn_points[1] self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if self.vehicle is None: -<<<<<<< HEAD + raise RuntimeError("无法生成车辆") self.vehicle.set_autopilot(False) @@ -106,39 +75,22 @@ def follow_vehicle(self): spectator.set_transform(carla.Transform(camera_location, camera_rotation)) def get_observation(self): -======= - raise RuntimeError("无法生成车辆，请检查spawn点是否有效或CARLA是否正常运行") - - # 禁用自动驾驶，由代码控制 - self.vehicle.set_autopilot(False) - self.world.tick() - - return self.get_observation() - def get_observation(self): - # 实际项目中需要添加相机传感器获取图像，这里先返回模拟数据 - # 示例：添加相机（简化版） ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 if self.camera is None: camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') -<<<<<<< HEAD - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) - self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) -======= - # 相机安装在车辆前方 + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - # 这里需要设置相机回调函数获取图像，简化起见返回随机图像 ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 + return np.random.randint(0, 256, size=(128, 128, 3), dtype=np.uint8) def step(self, action): if self.vehicle is None: raise RuntimeError("车辆未初始化，请先调用reset()") -<<<<<<< HEAD + throttle = 0.0 steer = 0.0 if action == 0: @@ -160,35 +112,12 @@ def step(self, action): next_state = self.get_observation() reward = 1.0 -======= - # 控制信号限制在有效范围（避免极端值） - throttle = 0.0 - steer = 0.0 - if action == 0: # 前进 - throttle = 0.5 # 适中油门，避免速度过快 - elif action == 1: # 左转 - throttle = 0.3 - steer = -0.5 - elif action == 2: # 右转 - throttle = 0.3 - steer = 0.5 - elif action == 3: # 后退 - throttle = -0.3 # 后退油门绝对值较小 - - self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer)) - self.world.tick() - - next_state = self.get_observation() - reward = 1.0 # 后续需根据任务设计奖励函数 ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 + done = False return next_state, reward, done, {} def close(self): -<<<<<<< HEAD -======= - # 清理所有生成的actor（车辆、相机等） ->>>>>>> 79105d613e987b049834579f1b08187777f87e95 + if self.camera is not None: self.camera.destroy() if self.vehicle is not None: From 65722a4cc61739fbdc67bcac7707dc75dc6286ba Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Tue, 2 Dec 2025 09:18:22 +0800 Subject: [PATCH 10/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index cf898d9d0a..be34b60007 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -117,7 +117,7 @@ def step(self, action): return next_state, reward, done, {} def close(self): - + if self.camera is not None: self.camera.destroy() if self.vehicle is not None: From b983225d79694c603cf2e6e6c25e20cb47000346 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Wed, 3 Dec 2025 22:47:13 +0800 Subject: [PATCH 11/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 71 ++++++++++++++++----------- 1 file changed, 42 insertions(+), 29 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index be34b60007..a2d120863a 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -1,22 +1,21 @@ import carla import numpy as np import gym +import random # 新增：用于随机选spawn点 +import time # 新增：用于销毁后延迟 class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() - self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8) self.vehicle = None - self.camera = None # 新增：镜头跟随参数（仅用于初始化跳转） @@ -24,14 +23,34 @@ def __init__(self): self.spectator_distance = -5.0 self.spectator_pitch = -10 + # 新增：初始化时先清理所有残留actor + self._clean_all_actors() + self.reset() - def reset(self): - + # 新增：核心清理函数 - 销毁所有残留的车辆/传感器/行人等actor + def _clean_all_actors(self): + # 获取当前世界所有actor + actor_list = self.world.get_actors() + for actor in actor_list: + # 筛选需要销毁的actor类型：车辆、传感器、行人（可根据需求调整） + if actor.type_id.startswith('vehicle') or actor.type_id.startswith('sensor') or actor.type_id.startswith('walker'): + try: + actor.destroy() + time.sleep(0.05) # 短暂延迟，确保销毁完成 + except Exception as e: + print(f"销毁actor失败: {e}") + # 额外清理当前实例的车辆和相机 + if self.camera is not None: + self.camera.destroy() + self.camera = None if self.vehicle is not None: self.vehicle.destroy() self.vehicle = None - + + def reset(self): + # 第一步：先清理残留车辆（增强版） + self._clean_all_actors() vehicle_bp = self.blueprint_library.filter('vehicle.*')[0] vehicle_bp.set_attribute('role_name', 'hero') @@ -40,17 +59,22 @@ def reset(self): if not spawn_points: spawn_point = carla.Transform(carla.Location(x=20, y=0, z=0.5)) else: - spawn_point = spawn_points[0] + # 改动1：随机打乱spawn点，避免固定选前几个 + random.shuffle(spawn_points) + + # 改动2：循环尝试多个spawn点（最多尝试10个），提高生成成功率 + self.vehicle = None + max_attempts = min(10, len(spawn_points)) # 最多试10个点（或所有点） + for i in range(max_attempts): + spawn_point = spawn_points[i] if spawn_points else carla.Transform(carla.Location(x=20, y=0, z=0.5)) + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if self.vehicle is not None: + break # 生成成功，退出循环 + time.sleep(0.1) # 失败后短暂延迟，再试下一个点 - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + # 最终检查：如果所有点都失败，抛出更友好的错误 if self.vehicle is None: - - if len(spawn_points) > 1: - spawn_point = spawn_points[1] - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) - if self.vehicle is None: - - raise RuntimeError("无法生成车辆") + raise RuntimeError(f"尝试了{max_attempts}个spawn点仍无法生成车辆，请检查CARLA模拟器状态或手动清理地图") self.vehicle.set_autopilot(False) self.world.tick() @@ -75,22 +99,18 @@ def follow_vehicle(self): spectator.set_transform(carla.Transform(camera_location, camera_rotation)) def get_observation(self): - if self.camera is None: camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - return np.random.randint(0, 256, size=(128, 128, 3), dtype=np.uint8) def step(self, action): if self.vehicle is None: raise RuntimeError("车辆未初始化，请先调用reset()") - throttle = 0.0 steer = 0.0 if action == 0: @@ -106,20 +126,13 @@ def step(self, action): self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer)) self.world.tick() - - # 已删除：step里的镜头跟随逻辑 → 后续车辆移动，镜头不再更新 - # self.follow_vehicle() # 这行已删掉 next_state = self.get_observation() reward = 1.0 - done = False return next_state, reward, done, {} def close(self): - - if self.camera is not None: - self.camera.destroy() - if self.vehicle is not None: - self.vehicle.destroy() - print("环境已清理") \ No newline at end of file + # 改动3：关闭时调用全局清理，确保无残留 + self._clean_all_actors() + print("环境已清理，所有actor已销毁") \ No newline at end of file From 57359b8fb82ca346c38993de1ee0a3d980233a73 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Mon, 8 Dec 2025 10:19:03 +0800 Subject: [PATCH 12/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 211 +++++++++++++------------- src/MMAP-DRL-Nav/run_simulation.py | 128 +++++++++++++--- 2 files changed, 214 insertions(+), 125 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index a5bc03017e..088a5a9246 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -1,160 +1,161 @@ import carla import numpy as np import gym -import random # 新增：用于随机选spawn点 -import time # 新增：用于销毁后延迟 +import time class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() + # 初始化CARLA客户端 self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() - - self.action_space = gym.spaces.Discrete(4) - self.observation_space = gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8) - + # 定义动作空间和观测空间 + self.action_space = gym.spaces.Discrete(4) # 前进、左转、右转、后退 + self.observation_space = gym.spaces.Box( + low=0, high=255, shape=(128, 128, 3), dtype=np.uint8 + ) # 128x128 RGB图像 + + # 核心对象初始化 self.vehicle = None + self.camera = None + self.image_data = None # 存储相机采集的图像数据 + # 镜头跟随参数（spectator视角） + self.spectator_offset = carla.Location(x=0, y=0, z=2.5) # 高度偏移 + self.spectator_distance = -5.0 # 车辆后方5米 + self.spectator_pitch = -10 # 向下俯视10度 - self.camera = None - - # 新增：镜头跟随参数（仅用于初始化跳转） - self.spectator_offset = carla.Location(x=0, y=0, z=2.5) - self.spectator_distance = -5.0 - self.spectator_pitch = -10 - - - # 新增：初始化时先清理所有残留actor - self._clean_all_actors() - - self.reset() - - # 新增：核心清理函数 - 销毁所有残留的车辆/传感器/行人等actor - def _clean_all_actors(self): - # 获取当前世界所有actor - actor_list = self.world.get_actors() - for actor in actor_list: - # 筛选需要销毁的actor类型：车辆、传感器、行人（可根据需求调整） - if actor.type_id.startswith('vehicle') or actor.type_id.startswith('sensor') or actor.type_id.startswith('walker'): - try: - actor.destroy() - time.sleep(0.05) # 短暂延迟，确保销毁完成 - except Exception as e: - print(f"销毁actor失败: {e}") - # 额外清理当前实例的车辆和相机 - if self.camera is not None: - self.camera.destroy() - self.camera = None + def reset(self): + """重置环境，生成新车辆和相机，返回初始观测""" + # 清理旧的车辆和相机资源 if self.vehicle is not None: self.vehicle.destroy() - self.vehicle = None - - def reset(self): - # 第一步：先清理残留车辆（增强版） - self._clean_all_actors() - + if self.camera is not None: + self.camera.destroy() + self.image_data = None - vehicle_bp = self.blueprint_library.filter('vehicle.*')[0] - vehicle_bp.set_attribute('role_name', 'hero') - + # 生成车辆（特斯拉Model3） + vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] spawn_points = self.world.get_map().get_spawn_points() - if not spawn_points: - spawn_point = carla.Transform(carla.Location(x=20, y=0, z=0.5)) - else: - - # 改动1：随机打乱spawn点，避免固定选前几个 - random.shuffle(spawn_points) - - # 改动2：循环尝试多个spawn点（最多尝试10个），提高生成成功率 - self.vehicle = None - max_attempts = min(10, len(spawn_points)) # 最多试10个点（或所有点） - for i in range(max_attempts): - spawn_point = spawn_points[i] if spawn_points else carla.Transform(carla.Location(x=20, y=0, z=0.5)) - self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) - if self.vehicle is not None: - break # 生成成功，退出循环 - time.sleep(0.1) # 失败后短暂延迟，再试下一个点 - - # 最终检查：如果所有点都失败，抛出更友好的错误 - if self.vehicle is None: - raise RuntimeError(f"尝试了{max_attempts}个spawn点仍无法生成车辆，请检查CARLA模拟器状态或手动清理地图") - - + spawn_point = spawn_points[0] if spawn_points else carla.Transform(carla.Location(x=0, y=0, z=0)) + self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) self.vehicle.set_autopilot(False) - self.world.tick() - # 仅保留：车辆生成时，镜头跳转到车辆旁（核心需求） + # 初始化RGB相机 + self._init_camera() + + # 等待相机采集到第一帧数据 + while self.image_data is None: + time.sleep(0.01) + + # 镜头跳转到车辆位置并跟随 self.follow_vehicle() - - return self.get_observation() - # 镜头跳转核心方法（仅初始化时调用一次） + self.world.tick() + return self.image_data.copy() + + def _init_camera(self): + """初始化挂载在车辆上的RGB相机""" + # 创建相机蓝图并设置参数 + camera_bp = self.blueprint_library.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '128') + camera_bp.set_attribute('image_size_y', '128') + camera_bp.set_attribute('fov', '90') # 视野角度 + + # 相机挂载位置：车辆前上方（x=1.5米，z=2.0米） + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) + self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) + + # 注册相机数据回调函数 + self.camera.listen(lambda img: self._camera_callback(img)) + + def _camera_callback(self, image): + """相机数据回调：将原始数据转换为RGB numpy数组""" + # CARLA相机输出为RGBA格式（4通道），转换为RGB（3通道） + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((image.height, image.width, 4)) + self.image_data = array[:, :, :3] # 去掉Alpha通道 + def follow_vehicle(self): + """调整spectator视角，让镜头跟随车辆""" spectator = self.world.get_spectator() if not spectator or not self.vehicle: return + + # 计算镜头位置（车辆后方+高度偏移） vehicle_transform = self.vehicle.get_transform() camera_location = vehicle_transform.location + carla.Location(x=self.spectator_distance) + self.spectator_offset + # 计算镜头朝向（与车辆一致，向下俯视） camera_rotation = carla.Rotation( pitch=self.spectator_pitch, yaw=vehicle_transform.rotation.yaw, roll=0 ) + # 设置镜头位置和朝向 spectator.set_transform(carla.Transform(camera_location, camera_rotation)) def get_observation(self): - - - if self.camera is None: - camera_bp = self.blueprint_library.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '128') - camera_bp.set_attribute('image_size_y', '128') - - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) - self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - - return np.random.randint(0, 256, size=(128, 128, 3), dtype=np.uint8) + """获取当前相机图像（模型输入的观测值）""" + if self.image_data is not None: + return self.image_data.copy() + # 兜底：相机未就绪时返回全零图像 + return np.zeros((128, 128, 3), dtype=np.uint8) def step(self, action): + """执行动作，返回新状态、奖励、终止标志等""" if self.vehicle is None: raise RuntimeError("车辆未初始化，请先调用reset()") - - + # 将动作映射为车辆控制指令 throttle = 0.0 steer = 0.0 - if action == 0: + if action == 0: # 前进 + throttle = 1.0 + elif action == 1: # 左转 + throttle = 0.5 + steer = -1.0 + elif action == 2: # 右转 throttle = 0.5 - elif action == 1: - throttle = 0.3 - steer = -0.5 - elif action == 2: - throttle = 0.3 - steer = 0.5 - elif action == 3: - throttle = -0.3 - + steer = 1.0 + elif action == 3: # 后退 + throttle = -1.0 + + # 应用车辆控制 self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer)) self.world.tick() - # 已删除：step里的镜头跟随逻辑 → 后续车辆移动，镜头不再更新 - # self.follow_vehicle() # 这行已删掉 - - next_state = self.get_observation() - reward = 1.0 + # 镜头实时跟随车辆 + self.follow_vehicle() + # 获取新状态、计算奖励、判断终止 + next_state = self.get_observation() + reward = self._calculate_reward(throttle) # 自定义奖励函数 + done = self._check_done() # 自定义终止条件 - done = False return next_state, reward, done, {} - def close(self): + def _calculate_reward(self, throttle): + """奖励函数：鼓励前进，惩罚后退""" + if throttle > 0: + return 0.1 # 前进奖励 + elif throttle < 0: + return -0.1 # 后退惩罚 + return 0.0 # 无动作无奖励 - # 改动3：关闭时调用全局清理，确保无残留 - self._clean_all_actors() - print("环境已清理，所有actor已销毁") + def _check_done(self): + """终止条件：示例为永不终止（可根据需求修改）""" + # 可扩展：碰撞检测、到达目标、超时等 + return False + + def close(self): + """关闭环境，清理所有资源""" + if self.vehicle is not None: + self.vehicle.destroy() + if self.camera is not None: + self.camera.destroy() + print("CARLA环境已关闭，资源清理完成") diff --git a/src/MMAP-DRL-Nav/run_simulation.py b/src/MMAP-DRL-Nav/run_simulation.py index 3f72c6fb5b..a5ebd56d33 100644 --- a/src/MMAP-DRL-Nav/run_simulation.py +++ b/src/MMAP-DRL-Nav/run_simulation.py @@ -1,6 +1,9 @@ -# 1. 导入模块（放在最开头） +# 1. 导入模块（新增 cv2 和 os 用于图像保存） import torch import time +import numpy as np +import cv2 # 用于图像保存 +import os # 用于创建目录 from models.perception_module import PerceptionModule from models.attention_module import CrossDomainAttention from models.decision_module import DecisionModule @@ -26,28 +29,113 @@ def forward(self, image, lidar_data, imu_data): policy, value = self.decision(fused_features) return policy, value -# 3. 定义 run_simulation 函数 -def run_simulation(): - env = CarlaEnvironment() # 初始化CARLA环境 - system = IntegratedSystem(device='cuda' if torch.cuda.is_available() else 'cpu') +# 3. 定义传感器数据适配函数（桥接CARLA和模型） +def adapt_sensor_data(env, system): + """ + 从CARLA环境获取真实图像，转换为模型输入格式 + （LiDAR/IMU暂用模拟数据，后续可扩展为真实传感器） + """ + # 1. 获取CARLA真实相机图像 (128, 128, 3) → 适配模型输入 + raw_image = env.get_observation() # 真实RGB图像 + # 转换格式：HWC(128,128,3) → CHW(3,128,128) → 缩放至256×256（匹配模型输入） + image = torch.FloatTensor(raw_image).permute(2, 0, 1).unsqueeze(0) / 255.0 # 归一化到[0,1] + image = torch.nn.functional.interpolate(image, size=(256, 256), mode='bilinear') # 缩放至256×256 + image = image.to(system.device) + + # 2. 模拟LiDAR数据（后续需添加CARLA LiDAR传感器） + lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(system.device) + + # 3. 模拟IMU数据（后续需添加CARLA IMU传感器） + imu_data = torch.randn(1, 6).to(system.device) - for _ in range(100): # 运行100步仿真 - # 生成随机传感器数据（实际中应从env获取真实数据） - image = torch.randn(3, 256, 256).unsqueeze(0).to(system.device) - lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(system.device) - imu_data = torch.randn(1, 6).to(system.device) + return image, lidar_data, imu_data, raw_image # 新增返回原始图像 - # 前向传播得到策略 - policy, value = system.forward(image, lidar_data, imu_data) - - # 转换为CARLA控制信号（限制范围避免异常） - throttle = float(torch.clamp(policy[0][0], 0, 1)) # 油门范围[0,1] - steer = float(torch.clamp(policy[0][1], -1, 1)) # 转向范围[-1,1] - control = carla.VehicleControl(throttle=throttle, steer=steer) - env.vehicle.apply_control(control) +# 4. 定义图像保存函数 +def save_camera_image(raw_image, step, save_dir="carla_camera_images"): + """ + 保存CARLA相机原始图像到本地 + :param raw_image: 原始RGB图像 (128, 128, 3) + :param step: 仿真步数 + :param save_dir: 保存目录 + """ + # 创建保存目录（不存在则创建） + if not os.path.exists(save_dir): + os.makedirs(save_dir) + + # 转换RGB格式为OpenCV的BGR格式（OpenCV默认BGR） + image_bgr = cv2.cvtColor(raw_image, cv2.COLOR_RGB2BGR) + + # 定义保存路径 + save_path = os.path.join(save_dir, f"camera_step_{step:03d}.png") # 03d 补零，如 001, 002 + + # 保存图像 + cv2.imwrite(save_path, image_bgr) + + # 打印保存日志 + print(f"📸 第 {step} 步相机图像已保存：{save_path}") - time.sleep(0.1) # 模拟时间间隔 +# 5. 定义 run_simulation 函数 +def run_simulation(): + # 初始化CARLA环境 + env = CarlaEnvironment() + # 关键：调用reset()生成车辆和相机，初始化self.vehicle + env.reset() + + # 校验车辆是否生成成功 + if env.vehicle is None: + raise RuntimeError("❌ 车辆生成失败！请检查：\n1. CARLA模拟器是否启动\n2. 端口是否为2000\n3. 地图是否加载完成") + + # 初始化集成系统 + system = IntegratedSystem(device='cuda' if torch.cuda.is_available() else 'cpu') + + print("✅ 仿真开始，运行100步...") + # 控制保存频率：比如每5步保存一张，或只保存前10张，避免文件过多 + save_frequency = 5 # 每5步保存一次 + max_save_images = 10 # 最多保存10张图像 + + saved_count = 0 + for step in range(100): + try: + # 获取适配后的传感器数据 + 原始图像 + image, lidar_data, imu_data, raw_image = adapt_sensor_data(env, system) + + # 保存相机图像（按频率保存，且不超过最大数量） + if (step + 1) % save_frequency == 0 and saved_count < max_save_images: + save_camera_image(raw_image, step + 1) + saved_count += 1 + + # 前向传播得到策略 + policy, value = system.forward(image, lidar_data, imu_data) + + # 转换为CARLA控制信号（限制范围避免异常） + throttle = float(torch.clamp(policy[0][0], 0, 1)) # 油门范围[0,1] + steer = float(torch.clamp(policy[0][1], -1, 1)) # 转向范围[-1,1] + control = carla.VehicleControl(throttle=throttle, steer=steer) + + # 应用控制指令到车辆 + env.vehicle.apply_control(control) + + # 打印运行日志（方便调试） + print(f"第 {step+1:3d} 步 | 油门：{throttle:.2f} | 转向：{steer:.2f} | 相机图像像素范围：{raw_image.min()}~{raw_image.max()}") + + time.sleep(0.1) # 模拟时间间隔 + + except Exception as e: + print(f"❌ 第 {step+1} 步出错：{str(e)}") + break + + # 仿真结束，清理环境 + env.close() + print("✅ 仿真结束，环境已清理") + print(f"📂 共保存 {saved_count} 张相机图像到：carla_camera_images/ 目录") -# 4. 程序入口（放在最后） +# 6. 程序入口（放在最后） if __name__ == "__main__": + # 检查OpenCV是否安装 + try: + import cv2 + except ImportError: + print("❌ 未安装OpenCV，无法保存图像！请执行：pip install opencv-python") + exit(1) + run_simulation() \ No newline at end of file From 73d1bf0d69696335b97097aa477931e1976a730c Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Wed, 10 Dec 2025 16:31:10 +0800 Subject: [PATCH 13/18] updata --- src/MMAP-DRL-Nav/dqn_agent.py | 169 +++++++++++++++++++++---------- src/MMAP-DRL-Nav/main.py | 146 ++++++++++++++++++++++++++ src/MMAP-DRL-Nav/pruning.py | 34 +++++++ src/MMAP-DRL-Nav/quantization.py | 7 ++ src/MMAP-DRL-Nav/train.py | 144 ++++++++++++++++++++++++++ 5 files changed, 448 insertions(+), 52 deletions(-) create mode 100644 src/MMAP-DRL-Nav/main.py create mode 100644 src/MMAP-DRL-Nav/pruning.py create mode 100644 src/MMAP-DRL-Nav/quantization.py create mode 100644 src/MMAP-DRL-Nav/train.py diff --git a/src/MMAP-DRL-Nav/dqn_agent.py b/src/MMAP-DRL-Nav/dqn_agent.py index 2ef914b102..b0975981ba 100644 --- a/src/MMAP-DRL-Nav/dqn_agent.py +++ b/src/MMAP-DRL-Nav/dqn_agent.py @@ -1,87 +1,152 @@ import torch import torch.nn as nn import numpy as np +import random +from collections import deque # 高效内存操作 from .pruning import prune_model from .quantization import quantize_model + class DQNAgent: - def __init__(self, state_size, action_size, config): - self.state_size = state_size + def __init__(self, state_shape, action_size, config): + """ + 初始化DQN智能体（适配CARLA图像输入，修复卷积维度计算错误） + :param state_shape: 图像形状 (128, 128, 3) + :param action_size: 动作维度（4：前进/左转/右转/后退） + :param config: 配置字典 + """ + self.state_shape = state_shape # (128, 128, 3) self.action_size = action_size - self.memory = [] + self.memory = deque(maxlen=config.get('agent', {}).get('memory_capacity', 10000)) # 经验池 self.gamma = 0.95 # 折扣因子 - self.epsilon = 1.0 # 探索率 - self.epsilon_decay = config['agent']['epsilon_decay'] - self.epsilon_min = config['agent']['epsilon_min'] - self.learning_rate = config['train']['learning_rate'] - self.model = self._build_model() + self.epsilon = 1.0 # 初始探索率 + self.epsilon_decay = config['agent']['epsilon_decay'] # 探索率衰减 + self.epsilon_min = config['agent']['epsilon_min'] # 最小探索率 + self.learning_rate = config['train']['learning_rate'] # 学习率 + self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") # 设备 + + # 构建CNN模型（自适应池化解决维度计算错误）并移到指定设备 + self.model = self._build_model().to(self.device) + self.optimizer = torch.optim.Adam(self.model.parameters(), lr=self.learning_rate) # 优化器 + self.loss_fn = nn.MSELoss() # 损失函数 + + # 模型输入输出校验（新增：验证维度匹配） + self._validate_model_dim() def _build_model(self): - model = nn.Sequential( - nn.Linear(self.state_size, 24), + """构建适配128×128×3图像的CNN模型（自适应池化，无需手动计算卷积维度）""" + return nn.Sequential( + # 卷积层1：提取低级视觉特征 (3,128,128) → (32,31,31) + nn.Conv2d(in_channels=3, out_channels=32, kernel_size=8, stride=4, padding=1), + nn.ReLU(), + # 卷积层2：提取中级特征 (32,31,31) → (64,15,15) + nn.Conv2d(32, 64, kernel_size=4, stride=2, padding=1), nn.ReLU(), - nn.Linear(24, 24), + # 卷积层3：提取高级特征 (64,15,15) → (64,15,15) + nn.Conv2d(64, 64, kernel_size=3, stride=1, padding=1), nn.ReLU(), - nn.Linear(24, self.action_size) + # 自适应池化：强制输出8×8，彻底避免维度计算错误 (64,15,15) → (64,8,8) + nn.AdaptiveAvgPool2d((8, 8)), + # 展平特征图（64*8*8=4096，固定维度） + nn.Flatten(), + # 全连接层：映射到动作空间 + nn.Linear(64 * 8 * 8, 512), + nn.ReLU(), + nn.Linear(512, self.action_size) # 输出4个动作的Q值 ) - return model + + def _validate_model_dim(self): + """验证模型输入输出维度是否匹配（新增）""" + try: + # 构建测试输入：(batch, 3, H, W) + dummy_input = torch.randn(1, 3, self.state_shape[0], self.state_shape[1]).to(self.device) + with torch.no_grad(): + dummy_output = self.model(dummy_input) + print(f"✅ 模型维度校验通过 | 输入维度：{dummy_input.shape} | 输出维度：{dummy_output.shape}") + except Exception as e: + raise ValueError(f"❌ 模型维度校验失败：{e}") def remember(self, state, action, reward, next_state, done): + """存储经验到回放池（标准化数据格式）""" + state = np.array(state, dtype=np.float32) + next_state = np.array(next_state, dtype=np.float32) + reward = np.array(reward, dtype=np.float32) self.memory.append((state, action, reward, next_state, done)) def act(self, state): + """ε-贪心策略选择动作（适配图像输入）""" + # 探索阶段：随机选动作 if np.random.rand() <= self.epsilon: return np.random.choice(self.action_size) - q_values = self.model(torch.FloatTensor(state)) - return np.argmax(q_values.detach().numpy()) + + # 利用阶段：模型预测最优动作 + # 维度转换：HWC(128,128,3) → CHW(3,128,128) + batch维度 + 归一化 + state_tensor = torch.FloatTensor(state).permute(2, 0, 1).unsqueeze(0).to(self.device) / 255.0 + # 模型推理（无梯度） + with torch.no_grad(): + q_values = self.model(state_tensor) + # 返回Q值最大的动作 + return np.argmax(q_values.cpu().detach().numpy()[0]) def replay(self, batch_size): - minibatch = np.random.choice(self.memory, batch_size) - for state, action, reward, next_state, done in minibatch: - target = reward - if not done: - target += self.gamma * np.amax(self.model(torch.FloatTensor(next_state)).detach().numpy()) - target_f = self.model(torch.FloatTensor(state)) - target_f[action] = target - self.model.fit(torch.FloatTensor(state), target_f.unsqueeze(0), epochs=1, verbose=0) + """批量经验回放（GPU加速，替换逐样本更新）""" + if len(self.memory) < batch_size: + return + + # 随机采样批次经验 + minibatch = random.sample(self.memory, batch_size) + # 拆分批次数据 + states = np.array([exp[0] for exp in minibatch]) # (batch, 128, 128, 3) + actions = np.array([exp[1] for exp in minibatch]) + rewards = np.array([exp[2] for exp in minibatch]) + next_states = np.array([exp[3] for exp in minibatch]) # (batch, 128, 128, 3) + dones = np.array([exp[4] for exp in minibatch]) + + # 维度转换：HWC → CHW + 移到设备 + 归一化 + states_tensor = torch.FloatTensor(states).permute(0, 3, 1, 2).to(self.device) / 255.0 # (batch, 3, 128, 128) + next_states_tensor = torch.FloatTensor(next_states).permute(0, 3, 1, 2).to(self.device) / 255.0 + actions_tensor = torch.LongTensor(actions).to(self.device) + rewards_tensor = torch.FloatTensor(rewards).to(self.device) + dones_tensor = torch.FloatTensor(dones).to(self.device) + + # 计算当前Q值（仅选执行动作的Q值） + current_q = self.model(states_tensor).gather(1, actions_tensor.unsqueeze(1)).squeeze(1) + + # 计算目标Q值（Bellman方程） + with torch.no_grad(): + next_q = self.model(next_states_tensor).max(1)[0] # 下一个状态的最大Q值 + target_q = rewards_tensor + self.gamma * next_q * (1 - dones_tensor) # 目标Q值 + + # 梯度下降更新 + self.optimizer.zero_grad() + loss = self.loss_fn(current_q, target_q) + loss.backward() + self.optimizer.step() + + # 衰减探索率 if self.epsilon > self.epsilon_min: self.epsilon *= self.epsilon_decay def calculate_reward(self, current_position, target_position, road_position, done): - #计算奖励 - #:param current_position: 当前机器人位置 (x, y) - #:param target_position: 目标位置 (x, y) - #:param road_position: 机器人在道路上的位置 - #:param done: 是否完成任务 - #:return: 奖励值 + """奖励函数（适配CARLA环境）""" distance_to_target = np.linalg.norm(current_position - target_position) distance_to_road = np.linalg.norm(current_position - road_position) if done: - return 100 # 到达目标的奖励 - elif distance_to_target < 1.0: # 靠近目标位置 - return 10 # 靠近目标的奖励 - elif distance_to_road > 1.0: # 远离道路 - return -5 # 远离道路的惩罚 - elif distance_to_target < 5.0: # 在某个距离范围内 - return 1 # 轻微奖励 + return 100.0 + elif distance_to_target < 1.0: + return 10.0 + elif distance_to_road > 1.0: + return -5.0 + elif distance_to_target < 5.0: + return 1.0 else: - return -1 # 远离目标的惩罚 + return -1.0 def get_state(self, position, orientation, target_position, road_position): - - #获取状态 - #:param position: 机器人当前位置 (x, y) - #:param orientation: 机器人朝向 (angle) - #:param target_position: 目标位置 (x, y) - #:param road_position: 机器人在道路上的位置 - # :return: 状态数组 + """备用：低维状态提取（实际训练用图像）""" state = np.array([ - position[0], # 当前 x 坐标 - position[1], # 当前 y 坐标 - orientation, # 当前朝向 - target_position[0], # 目标 x 坐标 - target_position[1], # 目标 y 坐标 - road_position[0], # 道路 x 坐标 - road_position[1] # 道路 y 坐标 - ]) + position[0], position[1], orientation, + target_position[0], target_position[1], + road_position[0], road_position[1] + ], dtype=np.float32) return state diff --git a/src/MMAP-DRL-Nav/main.py b/src/MMAP-DRL-Nav/main.py new file mode 100644 index 0000000000..18905108b4 --- /dev/null +++ b/src/MMAP-DRL-Nav/main.py @@ -0,0 +1,146 @@ +import torch +import torch.nn as nn +import torch.optim as optim +import numpy as np +import argparse +from models.dqn_agent import DQNAgent +from models.pruning import ModelPruner +from models.quantization import quantize_model +from envs.carla_environment import CarlaEnvironment +import yaml + +# -------------------------- +# 解析命令行参数（保留原有逻辑） +# -------------------------- +def parse_args(): + parser = argparse.ArgumentParser(description='CARLA DQN 训练/测试脚本') + parser.add_argument('--mode', type=str, required=True, choices=['train', 'test'], + help='运行模式：train（训练）/ test（测试）') + parser.add_argument('--config', type=str, default='configs/config.yaml', + help='配置文件路径') + return parser.parse_args() + +def load_config(config_path='configs/config.yaml'): + try: + with open(config_path, 'r') as file: + config = yaml.safe_load(file) + print(f"成功加载配置文件：{config_path}") + return config + except Exception as e: + print(f"加载配置文件失败：{e}") + raise + +def train_model(config): + print("=== 开始DQN训练 ===") + try: + # 初始化环境 + print("初始化CARLA环境...") + env = CarlaEnvironment() + print("CARLA环境初始化成功（车辆已生成）") + + # 关键修复1：获取完整图像形状，而非单维度 + state_shape = env.observation_space.shape # (128, 128, 3) + action_size = env.action_space.n + print(f"状态形状：{state_shape}，动作维度：{action_size}") # 修正打印文案 + + # 关键修复2：传state_shape参数，匹配新版DQN + agent = DQNAgent(state_shape=state_shape, action_size=action_size, config=config) + print("DQN智能体初始化成功") + + # 优化器（新版DQN已内置优化器，此处可注释/保留，避免重复初始化） + # optimizer = optim.Adam(agent.model.parameters(), lr=config['train']['learning_rate']) + # criterion = nn.MSELoss() + print(f"优化器初始化成功（学习率：{config['train']['learning_rate']}）") + + episodes = config['train']['episodes'] + print(f"开始训练：共{episodes}轮Episode") + for e in range(episodes): + state = env.reset() + state = state.astype(np.float32) / 255.0 # 新增：图像归一化 + done = False + total_reward = 0 + step = 0 + + while not done and step < 500: # 新增：限制步数，避免死循环 + step += 1 + action = agent.act(state) + next_state, reward, done, _ = env.step(action) + + # 数据预处理 + next_state = next_state.astype(np.float32) / 255.0 # 归一化 + reward = np.clip(reward, -10, 10) # 奖励裁剪 + + # 记忆 + agent.remember(state, action, reward, next_state, done) + state = next_state + total_reward += reward + + # 经验回放 + if len(agent.memory) > config['train']['batch_size']: + agent.replay(config['train']['batch_size']) + + # 打印轮次日志 + if (e + 1) % 5 == 0: + print(f"Episode {e+1:4d}/{episodes}, Total Reward: {total_reward:6.1f}, 探索率: {agent.epsilon:.4f}") + + # 剪枝和量化 + print("开始模型剪枝...") + pruner = ModelPruner(agent.model) + pruner.prune_model(amount=0.2) + print("模型剪枝完成（移除20%权重）") + + print("开始模型量化...") + agent.model = quantize_model(agent.model) + print("模型量化完成") + + # 导出模型为 ONNX 格式（关键修复：适配图像输入维度） + print("导出模型为ONNX格式...") + export_to_onnx(agent.model, state_shape, config.get('model', {}).get('onnx_path', 'model.onnx')) + print("模型导出成功！") + + except Exception as e: + print(f"训练过程出错：{e}") + raise + +# 关键修复：适配图像输入的ONNX导出函数 +def export_to_onnx(model, state_shape, file_path='model.onnx'): + # 图像输入维度：(1, 3, H, W)，匹配CNN输入 + dummy_input = torch.randn(1, 3, state_shape[0], state_shape[1]).to(next(model.parameters()).device) + try: + torch.onnx.export( + model, + dummy_input, + file_path, + opset_version=12, + input_names=["input_image"], + output_names=["action_q_values"], + dynamic_axes={"input_image": {0: "batch_size"}, "action_q_values": {0: "batch_size"}} + ) + except Exception as e: + print(f"ONNX导出失败：{e}") + raise + +# 测试函数（保留） +def test_model(config): + print("=== 开始测试 ===") + print("测试功能尚未实现，请补充代码后使用") + +# -------------------------- +# 程序入口（保留原有命令行逻辑） +# -------------------------- +if __name__ == "__main__": + try: + args = parse_args() + print(f"当前运行模式：{args.mode}") + + if args.mode == 'train': + config = load_config(args.config) + train_model(config) + elif args.mode == 'test': + config = load_config(args.config) + test_model(config) + else: + print(f"无效模式：{args.mode}，仅支持 train / test") + except Exception as e: + print(f"\n程序异常退出：{e}") + exit(1) diff --git a/src/MMAP-DRL-Nav/pruning.py b/src/MMAP-DRL-Nav/pruning.py new file mode 100644 index 0000000000..d9215cd453 --- /dev/null +++ b/src/MMAP-DRL-Nav/pruning.py @@ -0,0 +1,34 @@ +import torch +import torch.nn as nn +import torch.nn.utils.prune as prune + +class ModelPruner: + def __init__(self, model): + + + self.model = model + + def prune_model(self, amount=0.2): + + for layer in self.model.modules(): + if isinstance(layer, nn.Linear): + prune.ln_structured(layer, name='weight', amount=amount, n=2, dim=0) + print(f"Pruned {amount*100:.1f}% of {layer} weights.") + return self.model + + def remove_pruning(self): + + for layer in self.model.modules(): + if isinstance(layer, nn.Linear): + prune.remove(layer, 'weight') + print(f"Removed pruning from {layer}.") + + def print_pruning_summary(self): + + for layer in self.model.modules(): + if isinstance(layer, nn.Linear): + pruning_amount = 1.0 - torch.count_nonzero(layer.weight) / layer.weight.numel() + print(f"{layer}: {pruning_amount*100:.1f}% pruned.") + + + diff --git a/src/MMAP-DRL-Nav/quantization.py b/src/MMAP-DRL-Nav/quantization.py new file mode 100644 index 0000000000..68c6776239 --- /dev/null +++ b/src/MMAP-DRL-Nav/quantization.py @@ -0,0 +1,7 @@ +import torch + +def quantize_model(model): #对模型进行动态量化 + model.eval() + with torch.no_grad(): + quantized_model = torch.quantization.quantize_dynamic(model, {nn.Linear}, dtype=torch.qint8) + return quantized_model diff --git a/src/MMAP-DRL-Nav/train.py b/src/MMAP-DRL-Nav/train.py new file mode 100644 index 0000000000..8ed9b49d73 --- /dev/null +++ b/src/MMAP-DRL-Nav/train.py @@ -0,0 +1,144 @@ +# 强制打印脚本标识，确认运行的是新版本 +print("=====================================") +print("✅ 运行的是最终版训练脚本（train_final.py）") +print("=====================================") + +import torch +import torch.nn as nn +import torch.optim as optim +import numpy as np +import yaml +from models.dqn_agent import DQNAgent +from models.pruning import ModelPruner +from models.quantization import quantize_model +from envs.carla_environment import CarlaEnvironment + +def load_config(config_path='configs/config.yaml'): + """加载配置文件""" + try: + with open(config_path, 'r') as file: + config = yaml.safe_load(file) + print(f"✅ 成功加载配置文件：{config_path}") + return config + except Exception as e: + raise ValueError(f"❌ 加载配置文件失败：{e}") + +def train_model(config): + """训练DQN模型（适配CARLA真实图像输入）""" + # 1. 初始化设备（GPU/CPU） + device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + print(f"✅ 使用设备：{device}") + + # 2. 初始化CARLA环境 + print("🔧 初始化CARLA环境...") + env = CarlaEnvironment() + state_shape = env.observation_space.shape # (128, 128, 3) 完整图像形状 + action_size = env.action_space.n + print(f"✅ CARLA环境初始化成功 | 状态形状：{state_shape} | 动作维度：{action_size}") + + # 3. 初始化DQN智能体（仅传state_shape，绝对不含state_size） + print("🔧 初始化DQN智能体...") + agent = DQNAgent( + state_shape=state_shape, # 唯一正确的参数名 + action_size=action_size, + config=config + ) + print("✅ DQN智能体初始化成功") + + # 4. 训练参数 + episodes = config['train']['episodes'] + batch_size = config['train']['batch_size'] + reward_history = [] # 记录奖励历史 + + # 5. 开始训练 + print(f"🚀 开始训练：共{episodes}轮Episode") + for e in range(episodes): + # 重置环境，获取初始状态 + state = env.reset() + # 图像归一化（0-255 → 0-1） + state = state.astype(np.float32) / 255.0 + done = False + total_reward = 0 + step = 0 + + while not done: + step += 1 + # 选择动作 + action = agent.act(state) + # 执行动作，获取环境反馈 + next_state, reward, done, _ = env.step(action) + + # 数据预处理 + next_state = next_state.astype(np.float32) / 255.0 # 归一化 + reward = np.clip(reward, -10, 10) # 奖励裁剪，避免极端值 + + # 存储经验 + agent.remember(state, action, reward, next_state, done) + # 更新状态 + state = next_state + total_reward += reward + + # 经验回放（批量更新） + if len(agent.memory) > batch_size: + agent.replay(batch_size) + + # 防止单轮步数过多 + if step > 500: + done = True + + # 记录奖励，打印训练日志 + reward_history.append(total_reward) + avg_reward = np.mean(reward_history[-10:]) if len(reward_history) >= 10 else total_reward + print(f"📊 Episode {e+1}/{episodes} | 总奖励：{total_reward:.2f} | 最近10轮平均：{avg_reward:.2f} | 探索率：{agent.epsilon:.4f}") + + # 6. 模型优化（剪枝+量化） + print("\n🔧 开始模型优化（剪枝+量化）...") + try: + pruner = ModelPruner(agent.model) + pruner.prune_model(amount=0.2) # 剪枝20%参数 + agent.model = quantize_model(agent.model) # 量化模型 + print("✅ 模型优化完成") + except Exception as e: + print(f"⚠️ 模型优化失败（可忽略）：{e}") + + # 7. 导出ONNX模型 + try: + export_to_onnx(agent.model, state_shape, device) + except Exception as e: + print(f"⚠️ ONNX导出失败（可忽略）：{e}") + + # 8. 保存模型权重 + torch.save(agent.model.state_dict(), "dqn_carla_model_final.pth") + print("✅ 模型权重已保存：dqn_carla_model_final.pth") + + # 9. 清理环境 + env.close() + print("\n🎉 训练完成！") + +def export_to_onnx(model, state_shape, device, file_path='model_final.onnx'): + """导出ONNX模型（适配CNN图像输入）""" + # 构建dummy input：(1, 3, 128, 128) + dummy_input = torch.randn(1, 3, state_shape[0], state_shape[1]).to(device) + # 导出ONNX + torch.onnx.export( + model, + dummy_input, + file_path, + opset_version=12, + input_names=["input_image"], + output_names=["action_q_values"], + dynamic_axes={"input_image": {0: "batch_size"}, "action_q_values": {0: "batch_size"}} + ) + print(f"✅ ONNX模型已导出：{file_path}") + +if __name__ == "__main__": + # 加载配置 + config = load_config() + # 启动训练 + try: + train_model(config) + except Exception as e: + print(f"❌ 训练过程出错：{e}") + import traceback + traceback.print_exc() # 打印详细错误栈 + raise \ No newline at end of file From 7d1c1f08c823d5fe765d1f95b9d5f6ca35d871c5 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Thu, 11 Dec 2025 23:03:49 +0800 Subject: [PATCH 14/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 206 +++++++++++++++----------- 1 file changed, 116 insertions(+), 90 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index 088a5a9246..c5aaa53ae2 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -12,150 +12,176 @@ def __init__(self): self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() - # 定义动作空间和观测空间 - self.action_space = gym.spaces.Discrete(4) # 前进、左转、右转、后退 + # ========== 同步模式核心配置（锁帧率，避免卡死） ========== + self.sync_settings = self.world.get_settings() + self.sync_settings.synchronous_mode = True # 启用同步模式 + self.sync_settings.fixed_delta_seconds = 1.0 / 30 # 锁30fps（平衡流畅+稳定） + self.sync_settings.no_rendering_mode = False # 启用渲染（保证视角可见） + self.world.apply_settings(self.sync_settings) + + # 动作/观测空间 + self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box( low=0, high=255, shape=(128, 128, 3), dtype=np.uint8 - ) # 128x128 RGB图像 + ) - # 核心对象初始化 + # 核心对象 self.vehicle = None self.camera = None - self.image_data = None # 存储相机采集的图像数据 + self.collision_sensor = None + self.image_data = None + self.has_collision = False - # 镜头跟随参数（spectator视角） - self.spectator_offset = carla.Location(x=0, y=0, z=2.5) # 高度偏移 - self.spectator_distance = -5.0 # 车辆后方5米 - self.spectator_pitch = -10 # 向下俯视10度 + # 视角参数（同步模式最优配置） + self.view_height = 3.8 # 超高视角（3.8米） + self.view_pitch = -6.0 # 仅俯视6度（接近平视） + self.view_distance = 4.5 # 正后方4.5米 def reset(self): - """重置环境，生成新车辆和相机，返回初始观测""" - # 清理旧的车辆和相机资源 - if self.vehicle is not None: + """重置环境（同步模式下安全初始化）""" + # 清理旧资源 + if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() - if self.camera is not None: + if self.camera is not None and self.camera.is_alive: self.camera.destroy() - self.image_data = None + if self.collision_sensor is not None and self.collision_sensor.is_alive: + self.collision_sensor.destroy() + self.image_data = None + self.has_collision = False - # 生成车辆（特斯拉Model3） + # 生成车辆（同步模式下容错处理） vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] spawn_points = self.world.get_map().get_spawn_points() - spawn_point = spawn_points[0] if spawn_points else carla.Transform(carla.Location(x=0, y=0, z=0)) + spawn_point = spawn_points[0] if spawn_points else carla.Transform(carla.Location(x=100, y=100, z=0.5)) self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) self.vehicle.set_autopilot(False) - # 初始化RGB相机 + # 初始化传感器（同步模式下低延迟配置） self._init_camera() + self._init_collision_sensor() - # 等待相机采集到第一帧数据 - while self.image_data is None: - time.sleep(0.01) + # 等待传感器就绪（同步模式下精确等待） + timeout = 0 + while self.image_data is None and timeout < 30: # 30帧超时（1秒） + self.world.tick() # 同步tick，保证传感器数据更新 + time.sleep(0.001) + timeout += 1 - # 镜头跳转到车辆位置并跟随 + # 同步模式下强制绑定视角（无延迟） self.follow_vehicle() - self.world.tick() - return self.image_data.copy() + return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) def _init_camera(self): - """初始化挂载在车辆上的RGB相机""" - # 创建相机蓝图并设置参数 + """同步模式下低延迟相机初始化""" camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') - camera_bp.set_attribute('fov', '90') # 视野角度 - - # 相机挂载位置：车辆前上方（x=1.5米，z=2.0米） + camera_bp.set_attribute('fov', '90') + camera_bp.set_attribute('sensor_tick', '0.0') # 同步模式下无传感器延迟 + # 相机挂载在车辆前方（不影响视角） camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - - # 注册相机数据回调函数 self.camera.listen(lambda img: self._camera_callback(img)) + def _init_collision_sensor(self): + """同步模式下碰撞传感器""" + collision_bp = self.blueprint_library.find('sensor.other.collision') + collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) + self.collision_sensor = self.world.spawn_actor( + collision_bp, collision_transform, attach_to=self.vehicle + ) + self.collision_sensor.listen(lambda event: self._collision_callback(event)) + + def _collision_callback(self, event): + """碰撞回调（同步模式下即时响应）""" + self.has_collision = True + def _camera_callback(self, image): - """相机数据回调：将原始数据转换为RGB numpy数组""" - # CARLA相机输出为RGBA格式（4通道），转换为RGB（3通道） + """相机回调（同步模式下无延迟处理）""" array = np.frombuffer(image.raw_data, dtype=np.uint8) - array = array.reshape((image.height, image.width, 4)) - self.image_data = array[:, :, :3] # 去掉Alpha通道 + self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] def follow_vehicle(self): - """调整spectator视角，让镜头跟随车辆""" + """同步模式下丝滑视角更新（每帧必更，无延迟）""" spectator = self.world.get_spectator() - if not spectator or not self.vehicle: + if not spectator or not self.vehicle or not self.vehicle.is_alive: return - # 计算镜头位置（车辆后方+高度偏移） - vehicle_transform = self.vehicle.get_transform() - camera_location = vehicle_transform.location + carla.Location(x=self.spectator_distance) + self.spectator_offset - # 计算镜头朝向（与车辆一致，向下俯视） - camera_rotation = carla.Rotation( - pitch=self.spectator_pitch, - yaw=vehicle_transform.rotation.yaw, - roll=0 - ) - # 设置镜头位置和朝向 - spectator.set_transform(carla.Transform(camera_location, camera_rotation)) + # 同步模式下精准计算正后方位置（无误差） + vehicle_tf = self.vehicle.get_transform() + yaw_rad = np.radians(vehicle_tf.rotation.yaw) + # 正后方绝对坐标（同步模式下无偏移） + cam_x = vehicle_tf.location.x - (np.cos(yaw_rad) * self.view_distance) + cam_y = vehicle_tf.location.y - (np.sin(yaw_rad) * self.view_distance) + cam_z = self.view_height + + # 同步模式下强制更新视角（无延迟） + spectator.set_transform(carla.Transform( + carla.Location(x=cam_x, y=cam_y, z=cam_z), + carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0) + )) def get_observation(self): - """获取当前相机图像（模型输入的观测值）""" - if self.image_data is not None: - return self.image_data.copy() - # 兜底：相机未就绪时返回全零图像 - return np.zeros((128, 128, 3), dtype=np.uint8) + """同步模式下即时获取观测""" + return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) def step(self, action): - """执行动作，返回新状态、奖励、终止标志等""" - if self.vehicle is None: - raise RuntimeError("车辆未初始化，请先调用reset()") + """同步模式下step（每帧同步，丝滑无延迟）""" + if self.vehicle is None or not self.vehicle.is_alive: + raise RuntimeError("车辆未初始化/已销毁，请先调用reset()") - # 将动作映射为车辆控制指令 + # 同步模式下极致平滑车辆控制（零抖动） throttle = 0.0 steer = 0.0 if action == 0: # 前进 - throttle = 1.0 + throttle = 0.5 # 超平缓加速（匹配30fps） elif action == 1: # 左转 - throttle = 0.5 - steer = -1.0 + throttle = 0.4 + steer = -0.1 # 微转向（零物理抖动） elif action == 2: # 右转 - throttle = 0.5 - steer = 1.0 + throttle = 0.4 + steer = 0.1 # 微转向 elif action == 3: # 后退 - throttle = -1.0 - - # 应用车辆控制 - self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer)) - self.world.tick() - - # 镜头实时跟随车辆 - self.follow_vehicle() - - # 获取新状态、计算奖励、判断终止 + throttle = -0.2 # 超平缓后退 + + # 同步模式下应用车辆控制（无物理波动） + self.vehicle.apply_control(carla.VehicleControl( + throttle=throttle, + steer=steer, + hand_brake=False, + reverse=(throttle < 0), + gear=1, + manual_gear_shift=True + )) + + # ========== 同步模式核心：先tick再更新视角（无延迟） ========== + self.world.tick() # 同步帧推进（30fps） + self.follow_vehicle() # 视角与帧同步更新（丝滑） + + # 同步模式下碰撞检测（即时响应） next_state = self.get_observation() - reward = self._calculate_reward(throttle) # 自定义奖励函数 - done = self._check_done() # 自定义终止条件 + reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) + done = self.has_collision return next_state, reward, done, {} - def _calculate_reward(self, throttle): - """奖励函数：鼓励前进，惩罚后退""" - if throttle > 0: - return 0.1 # 前进奖励 - elif throttle < 0: - return -0.1 # 后退惩罚 - return 0.0 # 无动作无奖励 - - def _check_done(self): - """终止条件：示例为永不终止（可根据需求修改）""" - # 可扩展：碰撞检测、到达目标、超时等 - return False - def close(self): - """关闭环境，清理所有资源""" - if self.vehicle is not None: + """同步模式下安全关闭（必做：恢复异步模式）""" + # 第一步：恢复CARLA异步模式（避免卡死） + self.sync_settings.synchronous_mode = False + self.world.apply_settings(self.sync_settings) + + # 第二步：销毁所有对象 + if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() - if self.camera is not None: + if self.camera is not None and self.camera.is_alive: self.camera.destroy() - print("CARLA环境已关闭，资源清理完成") + if self.collision_sensor is not None and self.collision_sensor.is_alive: + self.collision_sensor.destroy() + + # 第三步：延迟释放（同步模式下必要） + time.sleep(0.5) + print("CARLA环境已关闭（同步模式已恢复为异步）") From 171acfab43bba1146c3867db883c1f6f98e44440 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Fri, 12 Dec 2025 14:49:08 +0800 Subject: [PATCH 15/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 148 +++++++++++++++++--------- src/MMAP-DRL-Nav/main.py | 75 +++++++++---- 2 files changed, 150 insertions(+), 73 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index c5aaa53ae2..3dd127b282 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -2,21 +2,51 @@ import numpy as np import gym import time +import socket # 端口检测 class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() # 初始化CARLA客户端 self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) - self.world = self.client.get_world() + self.client.set_timeout(20.0) # 超时20秒 + + # ========== 1. 端口检测 + 重试连接 ========== + def is_port_used(port): + """检测端口是否被占用""" + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + try: + s.connect(('localhost', port)) + return True + except: + return False + finally: + s.close() + + if not is_port_used(2000): + raise RuntimeError("❌ 2000端口未被占用，请先启动CARLA模拟器") + + max_retry = 3 + retry_count = 0 + while retry_count < max_retry: + try: + # 加载CARLA默认地图（避免map not found） + self.world = self.client.get_world() + break + except RuntimeError as e: + retry_count += 1 + print(f"⚠️ 连接失败，重试第{retry_count}次...") + time.sleep(5) + else: + raise RuntimeError("❌ CARLA连接超时（3次重试失败），请检查模拟器是否正常启动") + self.blueprint_library = self.world.get_blueprint_library() - # ========== 同步模式核心配置（锁帧率，避免卡死） ========== + # ========== 同步模式配置（视角不抖核心） ========== self.sync_settings = self.world.get_settings() - self.sync_settings.synchronous_mode = True # 启用同步模式 - self.sync_settings.fixed_delta_seconds = 1.0 / 30 # 锁30fps（平衡流畅+稳定） - self.sync_settings.no_rendering_mode = False # 启用渲染（保证视角可见） + self.sync_settings.synchronous_mode = True # 同步模式是视角不抖的关键 + self.sync_settings.fixed_delta_seconds = 1.0 / 30 # 固定30fps，避免帧率波动 + self.sync_settings.no_rendering_mode = False self.world.apply_settings(self.sync_settings) # 动作/观测空间 @@ -32,13 +62,13 @@ def __init__(self): self.image_data = None self.has_collision = False - # 视角参数（同步模式最优配置） + # 视角参数（原无抖动配置，未修改） self.view_height = 3.8 # 超高视角（3.8米） self.view_pitch = -6.0 # 仅俯视6度（接近平视） self.view_distance = 4.5 # 正后方4.5米 def reset(self): - """重置环境（同步模式下安全初始化）""" + """重置环境（仅随机出生点，无地图切换/额外打印）""" # 清理旧资源 if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() @@ -49,43 +79,52 @@ def reset(self): self.image_data = None self.has_collision = False - # 生成车辆（同步模式下容错处理） + # ========== 仅随机选择出生点（无任何打印） ========== vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] spawn_points = self.world.get_map().get_spawn_points() - spawn_point = spawn_points[0] if spawn_points else carla.Transform(carla.Location(x=100, y=100, z=0.5)) + + # 随机选预设出生点（优先），无则随机生成坐标 + if len(spawn_points) > 0: + spawn_point = np.random.choice(spawn_points) + else: + # 随机生成合法范围坐标 + random_x = np.random.uniform(-200, 200) + random_y = np.random.uniform(-200, 200) + spawn_point = carla.Transform(carla.Location(x=random_x, y=random_y, z=0.5)) + + # 生成车辆 self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) self.vehicle.set_autopilot(False) - # 初始化传感器（同步模式下低延迟配置） + # 初始化传感器 self._init_camera() self._init_collision_sensor() - # 等待传感器就绪（同步模式下精确等待） + # 等待传感器就绪 timeout = 0 - while self.image_data is None and timeout < 30: # 30帧超时（1秒） - self.world.tick() # 同步tick，保证传感器数据更新 + while self.image_data is None and timeout < 30: + self.world.tick() # 同步tick，保证传感器稳定 time.sleep(0.001) timeout += 1 - # 同步模式下强制绑定视角（无延迟） + # 绑定视角（原无抖动逻辑） self.follow_vehicle() self.world.tick() return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) def _init_camera(self): - """同步模式下低延迟相机初始化""" + """初始化RGB相机（原无抖动配置）""" camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') camera_bp.set_attribute('fov', '90') - camera_bp.set_attribute('sensor_tick', '0.0') # 同步模式下无传感器延迟 - # 相机挂载在车辆前方（不影响视角） + camera_bp.set_attribute('sensor_tick', '0.0') # 同步模式下无延迟 camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) self.camera.listen(lambda img: self._camera_callback(img)) def _init_collision_sensor(self): - """同步模式下碰撞传感器""" + """初始化碰撞传感器（原配置）""" collision_bp = self.blueprint_library.find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) self.collision_sensor = self.world.spawn_actor( @@ -94,58 +133,56 @@ def _init_collision_sensor(self): self.collision_sensor.listen(lambda event: self._collision_callback(event)) def _collision_callback(self, event): - """碰撞回调（同步模式下即时响应）""" + """碰撞回调（原配置）""" self.has_collision = True def _camera_callback(self, image): - """相机回调（同步模式下无延迟处理）""" + """相机数据回调（原配置）""" array = np.frombuffer(image.raw_data, dtype=np.uint8) self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] def follow_vehicle(self): - """同步模式下丝滑视角更新（每帧必更，无延迟）""" + """视角跟随车辆（原无抖动核心逻辑，未做任何修改）""" spectator = self.world.get_spectator() if not spectator or not self.vehicle or not self.vehicle.is_alive: return - # 同步模式下精准计算正后方位置（无误差） + # 精准计算正后方视角（无抖动关键） vehicle_tf = self.vehicle.get_transform() yaw_rad = np.radians(vehicle_tf.rotation.yaw) - # 正后方绝对坐标（同步模式下无偏移） cam_x = vehicle_tf.location.x - (np.cos(yaw_rad) * self.view_distance) cam_y = vehicle_tf.location.y - (np.sin(yaw_rad) * self.view_distance) cam_z = self.view_height - # 同步模式下强制更新视角（无延迟） + # 强制设置视角（同步模式下无帧率波动，视角不抖） spectator.set_transform(carla.Transform( carla.Location(x=cam_x, y=cam_y, z=cam_z), carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0) )) def get_observation(self): - """同步模式下即时获取观测""" + """获取观测数据（原配置）""" return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) def step(self, action): - """同步模式下step（每帧同步，丝滑无延迟）""" + """执行单步动作（原无抖动逻辑，仅同步tick）""" if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁，请先调用reset()") - # 同步模式下极致平滑车辆控制（零抖动） + # 平滑车辆控制（原参数，无修改） throttle = 0.0 steer = 0.0 if action == 0: # 前进 - throttle = 0.5 # 超平缓加速（匹配30fps） + throttle = 0.5 elif action == 1: # 左转 throttle = 0.4 - steer = -0.1 # 微转向（零物理抖动） + steer = -0.1 elif action == 2: # 右转 throttle = 0.4 - steer = 0.1 # 微转向 + steer = 0.1 elif action == 3: # 后退 - throttle = -0.2 # 超平缓后退 + throttle = -0.2 - # 同步模式下应用车辆控制（无物理波动） self.vehicle.apply_control(carla.VehicleControl( throttle=throttle, steer=steer, @@ -155,11 +192,9 @@ def step(self, action): manual_gear_shift=True )) - # ========== 同步模式核心：先tick再更新视角（无延迟） ========== - self.world.tick() # 同步帧推进（30fps） - self.follow_vehicle() # 视角与帧同步更新（丝滑） + self.world.tick() # 同步帧推进（30fps，无帧率波动） + self.follow_vehicle() # 视角同步更新（无抖动） - # 同步模式下碰撞检测（即时响应） next_state = self.get_observation() reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) done = self.has_collision @@ -167,21 +202,32 @@ def step(self, action): return next_state, reward, done, {} def close(self): - """同步模式下安全关闭（必做：恢复异步模式）""" - # 第一步：恢复CARLA异步模式（避免卡死） - self.sync_settings.synchronous_mode = False - self.world.apply_settings(self.sync_settings) - - # 第二步：销毁所有对象 - if self.vehicle is not None and self.vehicle.is_alive: - self.vehicle.destroy() - if self.camera is not None and self.camera.is_alive: - self.camera.destroy() - if self.collision_sensor is not None and self.collision_sensor.is_alive: - self.collision_sensor.destroy() + """安全关闭环境（原配置+容错）""" + try: + self.sync_settings.synchronous_mode = False + self.world.apply_settings(self.sync_settings) + except Exception as e: + print(f"⚠️ 恢复异步模式时警告：{e}") + + try: + if self.vehicle is not None and self.vehicle.is_alive: + self.vehicle.destroy() + except Exception as e: + print(f"⚠️ 销毁车辆时警告：{e}") + + try: + if self.camera is not None and self.camera.is_alive: + self.camera.destroy() + except Exception as e: + print(f"⚠️ 销毁相机时警告：{e}") + + try: + if self.collision_sensor is not None and self.collision_sensor.is_alive: + self.collision_sensor.destroy() + except Exception as e: + print(f"⚠️ 销毁碰撞传感器时警告：{e}") - # 第三步：延迟释放（同步模式下必要） time.sleep(0.5) - print("CARLA环境已关闭（同步模式已恢复为异步）") + print("✅ CARLA环境已关闭（同步模式已恢复为异步）") diff --git a/src/MMAP-DRL-Nav/main.py b/src/MMAP-DRL-Nav/main.py index 18905108b4..513eef997d 100644 --- a/src/MMAP-DRL-Nav/main.py +++ b/src/MMAP-DRL-Nav/main.py @@ -10,7 +10,7 @@ import yaml # -------------------------- -# 解析命令行参数（保留原有逻辑） +# 解析命令行参数（保留你的逻辑） # -------------------------- def parse_args(): parser = argparse.ArgumentParser(description='CARLA DQN 训练/测试脚本') @@ -33,44 +33,42 @@ def load_config(config_path='configs/config.yaml'): def train_model(config): print("=== 开始DQN训练 ===") try: - # 初始化环境 + # 初始化环境（已集成无抖动后方摄像头） print("初始化CARLA环境...") env = CarlaEnvironment() - print("CARLA环境初始化成功（车辆已生成）") + print("CARLA环境初始化成功（车辆已生成，摄像头挂载完成）") - # 关键修复1：获取完整图像形状，而非单维度 - state_shape = env.observation_space.shape # (128, 128, 3) + # 关键：获取完整图像形状（128,128,3），适配新版DQN + state_shape = env.observation_space.shape action_size = env.action_space.n - print(f"状态形状：{state_shape}，动作维度：{action_size}") # 修正打印文案 + print(f"状态形状：{state_shape}，动作维度：{action_size}") - # 关键修复2：传state_shape参数，匹配新版DQN + # 初始化DQN（传state_shape，匹配新版DQN） agent = DQNAgent(state_shape=state_shape, action_size=action_size, config=config) print("DQN智能体初始化成功") - # 优化器（新版DQN已内置优化器，此处可注释/保留，避免重复初始化） - # optimizer = optim.Adam(agent.model.parameters(), lr=config['train']['learning_rate']) - # criterion = nn.MSELoss() + # 优化器（新版DQN已内置，此处仅打印日志） print(f"优化器初始化成功（学习率：{config['train']['learning_rate']}）") episodes = config['train']['episodes'] print(f"开始训练：共{episodes}轮Episode") for e in range(episodes): state = env.reset() - state = state.astype(np.float32) / 255.0 # 新增：图像归一化 + state = state.astype(np.float32) / 255.0 # 图像归一化 done = False total_reward = 0 step = 0 - while not done and step < 500: # 新增：限制步数，避免死循环 + while not done and step < 500: # 限制步数，避免死循环 step += 1 action = agent.act(state) next_state, reward, done, _ = env.step(action) # 数据预处理 - next_state = next_state.astype(np.float32) / 255.0 # 归一化 - reward = np.clip(reward, -10, 10) # 奖励裁剪 + next_state = next_state.astype(np.float32) / 255.0 + reward = np.clip(reward, -10, 10) - # 记忆 + # 记忆存储 agent.remember(state, action, reward, next_state, done) state = next_state total_reward += reward @@ -79,11 +77,11 @@ def train_model(config): if len(agent.memory) > config['train']['batch_size']: agent.replay(config['train']['batch_size']) - # 打印轮次日志 + # 打印训练日志（每5轮一次） if (e + 1) % 5 == 0: print(f"Episode {e+1:4d}/{episodes}, Total Reward: {total_reward:6.1f}, 探索率: {agent.epsilon:.4f}") - # 剪枝和量化 + # 模型优化（剪枝+量化） print("开始模型剪枝...") pruner = ModelPruner(agent.model) pruner.prune_model(amount=0.2) @@ -93,16 +91,20 @@ def train_model(config): agent.model = quantize_model(agent.model) print("模型量化完成") - # 导出模型为 ONNX 格式（关键修复：适配图像输入维度） + # 导出ONNX模型（适配图像输入） print("导出模型为ONNX格式...") export_to_onnx(agent.model, state_shape, config.get('model', {}).get('onnx_path', 'model.onnx')) print("模型导出成功！") + # 保存模型权重 + torch.save(agent.model.state_dict(), "dqn_carla_final.pth") + print("模型权重已保存：dqn_carla_final.pth") + except Exception as e: print(f"训练过程出错：{e}") raise -# 关键修复：适配图像输入的ONNX导出函数 +# 适配图像输入的ONNX导出函数 def export_to_onnx(model, state_shape, file_path='model.onnx'): # 图像输入维度：(1, 3, H, W)，匹配CNN输入 dummy_input = torch.randn(1, 3, state_shape[0], state_shape[1]).to(next(model.parameters()).device) @@ -123,10 +125,37 @@ def export_to_onnx(model, state_shape, file_path='model.onnx'): # 测试函数（保留） def test_model(config): print("=== 开始测试 ===") - print("测试功能尚未实现，请补充代码后使用") + try: + env = CarlaEnvironment() + state_shape = env.observation_space.shape + action_size = env.action_space.n + agent = DQNAgent(state_shape=state_shape, action_size=action_size, config=config) + # 加载训练好的模型 + agent.model.load_state_dict(torch.load("dqn_carla_final.pth")) + agent.model.eval() # 评估模式 + + print("开始测试（10轮）...") + for e in range(10): + state = env.reset() + state = state.astype(np.float32) / 255.0 + done = False + total_reward = 0 + step = 0 + while not done and step < 500: + step += 1 + action = agent.act(state) # 关闭探索，仅用模型预测 + next_state, reward, done, _ = env.step(action) + next_state = next_state.astype(np.float32) / 255.0 + state = next_state + total_reward += reward + print(f"Test Episode {e+1}, Total Reward: {total_reward:.1f}") + env.close() + except Exception as e: + print(f"测试过程出错：{e}") + raise # -------------------------- -# 程序入口（保留原有命令行逻辑） +# 程序入口（保留你的命令行逻辑） # -------------------------- if __name__ == "__main__": try: @@ -143,4 +172,6 @@ def test_model(config): print(f"无效模式：{args.mode}，仅支持 train / test") except Exception as e: print(f"\n程序异常退出：{e}") - exit(1) + import traceback + traceback.print_exc() # 打印详细错误栈 + exit(1) From d2a8be18279316877b4c4094c6518db4cfcaad0b Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Fri, 12 Dec 2025 18:56:45 +0800 Subject: [PATCH 16/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 79 +++++++++++++++++++++++++-- 1 file changed, 74 insertions(+), 5 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index 3dd127b282..b77bb4e2f8 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -3,6 +3,7 @@ import gym import time import socket # 端口检测 +import random # 仅新增：随机生成NPC位置/选择蓝图 class CarlaEnvironment(gym.Env): def __init__(self): @@ -49,6 +50,14 @@ def is_port_used(port): self.sync_settings.no_rendering_mode = False self.world.apply_settings(self.sync_settings) + # ========== 仅新增：NPC基础配置（不卡的少量数量） ========== + self.traffic_manager = self.client.get_trafficmanager(8000) + self.traffic_manager.set_synchronous_mode(True) + self.npc_vehicle_list = [] # 存储NPC车辆 + self.npc_pedestrian_list = [] # 存储NPC行人 + self.hit_vehicle = False # 撞车标记（终止） + self.hit_pedestrian = False # 撞人标记（不终止） + # 动作/观测空间 self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box( @@ -67,6 +76,41 @@ def is_port_used(port): self.view_pitch = -6.0 # 仅俯视6度（接近平视） self.view_distance = 4.5 # 正后方4.5米 + # ========== 仅新增：生成少量NPC（10车+5行人，保证不卡） ========== + def _spawn_small_npc(self): + # 清理旧NPC + for v in self.npc_vehicle_list: + if v.is_alive: + v.destroy() + self.npc_vehicle_list.clear() + for p in self.npc_pedestrian_list: + if p.is_alive: + p.destroy() + self.npc_pedestrian_list.clear() + + # 生成10辆NPC车辆（少量不卡） + vehicle_bps = self.blueprint_library.filter('vehicle.*') + spawn_points = self.world.get_map().get_spawn_points()[:10] # 仅取前10个生成点 + for sp in spawn_points: + try: + npc_vehicle = self.world.spawn_actor(random.choice(vehicle_bps), sp) + self.npc_vehicle_list.append(npc_vehicle) + npc_vehicle.set_autopilot(True, self.traffic_manager.get_port()) + except: + continue + + # 生成5个NPC行人（少量不卡） + pedestrian_bps = self.blueprint_library.filter('walker.pedestrian.*') + for _ in range(5): + # 随机生成行人位置（地图内合法范围） + loc = carla.Location(x=random.uniform(-100, 100), y=random.uniform(-100, 100), z=0) + try: + pedestrian = self.world.try_spawn_actor(random.choice(pedestrian_bps), carla.Transform(loc)) + if pedestrian: + self.npc_pedestrian_list.append(pedestrian) + except: + continue + def reset(self): """重置环境（仅随机出生点，无地图切换/额外打印）""" # 清理旧资源 @@ -79,6 +123,10 @@ def reset(self): self.image_data = None self.has_collision = False + # ========== 仅新增：重置碰撞标记 ========== + self.hit_vehicle = False + self.hit_pedestrian = False + # ========== 仅随机选择出生点（无任何打印） ========== vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] spawn_points = self.world.get_map().get_spawn_points() @@ -100,6 +148,9 @@ def reset(self): self._init_camera() self._init_collision_sensor() + # ========== 仅新增：调用生成少量NPC ========== + self._spawn_small_npc() + # 等待传感器就绪 timeout = 0 while self.image_data is None and timeout < 30: @@ -124,7 +175,7 @@ def _init_camera(self): self.camera.listen(lambda img: self._camera_callback(img)) def _init_collision_sensor(self): - """初始化碰撞传感器（原配置）""" + """初始化碰撞传感器（仅修改回调函数）""" collision_bp = self.blueprint_library.find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) self.collision_sensor = self.world.spawn_actor( @@ -132,9 +183,15 @@ def _init_collision_sensor(self): ) self.collision_sensor.listen(lambda event: self._collision_callback(event)) + # ========== 仅修改：碰撞回调（区分撞车/撞人） ========== def _collision_callback(self, event): - """碰撞回调（原配置）""" + """碰撞回调（区分撞车/撞人）""" self.has_collision = True + other_actor_type = event.other_actor.type_id + if 'vehicle' in other_actor_type: + self.hit_vehicle = True # 撞车标记 + elif 'walker' in other_actor_type: + self.hit_pedestrian = True # 撞人标记 def _camera_callback(self, image): """相机数据回调（原配置）""" @@ -165,7 +222,7 @@ def get_observation(self): return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) def step(self, action): - """执行单步动作（原无抖动逻辑，仅同步tick）""" + """执行单步动作（原无抖动逻辑，仅修改终止条件）""" if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁，请先调用reset()") @@ -197,12 +254,24 @@ def step(self, action): next_state = self.get_observation() reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) - done = self.has_collision + + # ========== 仅修改：终止条件（撞车才结束，撞人/其他碰撞不结束） ========== + done = self.hit_vehicle return next_state, reward, done, {} def close(self): - """安全关闭环境（原配置+容错）""" + """安全关闭环境（原配置+容错，仅新增清理NPC）""" + # ========== 仅新增：清理NPC ========== + for v in self.npc_vehicle_list: + if v.is_alive: + v.destroy() + for p in self.npc_pedestrian_list: + if p.is_alive: + p.destroy() + self.traffic_manager.set_synchronous_mode(False) + + # 原有清理逻辑（完全保留） try: self.sync_settings.synchronous_mode = False self.world.apply_settings(self.sync_settings) From 97e62db52acca35eda54c318a46f3974ef2e2fda Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Fri, 12 Dec 2025 21:55:50 +0800 Subject: [PATCH 17/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 132 +++++++++++++++----------- 1 file changed, 79 insertions(+), 53 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index b77bb4e2f8..9326ed3f37 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -50,7 +50,7 @@ def is_port_used(port): self.sync_settings.no_rendering_mode = False self.world.apply_settings(self.sync_settings) - # ========== 仅新增：NPC基础配置（不卡的少量数量） ========== + # ========== NPC配置（50辆车+5行人） ========== self.traffic_manager = self.client.get_trafficmanager(8000) self.traffic_manager.set_synchronous_mode(True) self.npc_vehicle_list = [] # 存储NPC车辆 @@ -71,12 +71,13 @@ def is_port_used(port): self.image_data = None self.has_collision = False - # 视角参数（原无抖动配置，未修改） - self.view_height = 3.8 # 超高视角（3.8米） - self.view_pitch = -6.0 # 仅俯视6度（接近平视） - self.view_distance = 4.5 # 正后方4.5米 + # 视角参数（核心修改：进一步后移视角，确保看到车屁股） + self.view_height = 6.0 # 基础高度保持6米（高视角） + self.view_pitch = -15.0 # 俯视角度保持15度（向下看） + self.view_distance = 8.0 # 正后方距离从6→8米（大幅后移，必看车屁股） + self.z_offset = 0.5 # z轴补偿保留，避免上坡卡地下 - # ========== 仅新增：生成少量NPC（10车+5行人，保证不卡） ========== + # ========== NPC生成逻辑（完全保留） ========== def _spawn_small_npc(self): # 清理旧NPC for v in self.npc_vehicle_list: @@ -88,9 +89,30 @@ def _spawn_small_npc(self): p.destroy() self.npc_pedestrian_list.clear() - # 生成10辆NPC车辆（少量不卡） + # 生成50辆NPC车辆（优化：过滤主角车辆附近的生成点，避免初始碰撞） vehicle_bps = self.blueprint_library.filter('vehicle.*') - spawn_points = self.world.get_map().get_spawn_points()[:10] # 仅取前10个生成点 + spawn_points = self.world.get_map().get_spawn_points() + + # 优化1：如果预设生成点不足50个，补充随机生成点 + if len(spawn_points) < 50: + for _ in range(50 - len(spawn_points)): + random_x = np.random.uniform(-200, 200) + random_y = np.random.uniform(-200, 200) + spawn_points.append(carla.Transform(carla.Location(x=random_x, y=random_y, z=0.5))) + + # 优化2：过滤距离主角车辆<15米的生成点，避免初始碰撞 + if self.vehicle is not None: + hero_loc = self.vehicle.get_transform().location + valid_spawn = [] + for sp in spawn_points: + dist = np.linalg.norm([sp.location.x - hero_loc.x, sp.location.y - hero_loc.y]) + if dist > 15.0: + valid_spawn.append(sp) + spawn_points = valid_spawn[:50] # 仅取前50个安全生成点 + else: + spawn_points = spawn_points[:50] + + # 生成50辆NPC车辆 for sp in spawn_points: try: npc_vehicle = self.world.spawn_actor(random.choice(vehicle_bps), sp) @@ -99,7 +121,7 @@ def _spawn_small_npc(self): except: continue - # 生成5个NPC行人（少量不卡） + # 生成5个NPC行人（数量不变） pedestrian_bps = self.blueprint_library.filter('walker.pedestrian.*') for _ in range(5): # 随机生成行人位置（地图内合法范围） @@ -111,8 +133,29 @@ def _spawn_small_npc(self): except: continue + # ========== 补全缺失的 _init_collision_sensor 方法（核心修复） ========== + def _init_collision_sensor(self): + """初始化碰撞传感器（原配置）""" + collision_bp = self.blueprint_library.find('sensor.other.collision') + collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) + self.collision_sensor = self.world.spawn_actor( + collision_bp, collision_transform, attach_to=self.vehicle + ) + self.collision_sensor.listen(lambda event: self._collision_callback(event)) + + def _init_camera(self): + """初始化RGB相机（完全保留）""" + camera_bp = self.blueprint_library.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '128') + camera_bp.set_attribute('image_size_y', '128') + camera_bp.set_attribute('fov', '90') + camera_bp.set_attribute('sensor_tick', '0.0') # 同步模式下无延迟 + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) + self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) + self.camera.listen(lambda img: self._camera_callback(img)) + def reset(self): - """重置环境（仅随机出生点，无地图切换/额外打印）""" + """重置环境（完全保留）""" # 清理旧资源 if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() @@ -123,11 +166,11 @@ def reset(self): self.image_data = None self.has_collision = False - # ========== 仅新增：重置碰撞标记 ========== + # 重置碰撞标记 self.hit_vehicle = False self.hit_pedestrian = False - # ========== 仅随机选择出生点（无任何打印） ========== + # 仅随机选择出生点（无任何打印） vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] spawn_points = self.world.get_map().get_spawn_points() @@ -144,11 +187,11 @@ def reset(self): self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) self.vehicle.set_autopilot(False) - # 初始化传感器 + # 初始化传感器（调用补全的方法） self._init_camera() self._init_collision_sensor() - # ========== 仅新增：调用生成少量NPC ========== + # 调用生成NPC self._spawn_small_npc() # 等待传感器就绪 @@ -158,34 +201,13 @@ def reset(self): time.sleep(0.001) timeout += 1 - # 绑定视角（原无抖动逻辑） + # 绑定视角（优化后的逻辑） self.follow_vehicle() self.world.tick() return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) - def _init_camera(self): - """初始化RGB相机（原无抖动配置）""" - camera_bp = self.blueprint_library.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '128') - camera_bp.set_attribute('image_size_y', '128') - camera_bp.set_attribute('fov', '90') - camera_bp.set_attribute('sensor_tick', '0.0') # 同步模式下无延迟 - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) - self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) - self.camera.listen(lambda img: self._camera_callback(img)) - - def _init_collision_sensor(self): - """初始化碰撞传感器（仅修改回调函数）""" - collision_bp = self.blueprint_library.find('sensor.other.collision') - collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) - self.collision_sensor = self.world.spawn_actor( - collision_bp, collision_transform, attach_to=self.vehicle - ) - self.collision_sensor.listen(lambda event: self._collision_callback(event)) - - # ========== 仅修改：碰撞回调（区分撞车/撞人） ========== def _collision_callback(self, event): - """碰撞回调（区分撞车/撞人）""" + """碰撞回调（完全保留）""" self.has_collision = True other_actor_type = event.other_actor.type_id if 'vehicle' in other_actor_type: @@ -194,39 +216,44 @@ def _collision_callback(self, event): self.hit_pedestrian = True # 撞人标记 def _camera_callback(self, image): - """相机数据回调（原配置）""" + """相机数据回调（完全保留）""" array = np.frombuffer(image.raw_data, dtype=np.uint8) self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] def follow_vehicle(self): - """视角跟随车辆（原无抖动核心逻辑，未做任何修改）""" + """视角跟随车辆（完全保留逻辑，仅参数变化）""" spectator = self.world.get_spectator() if not spectator or not self.vehicle or not self.vehicle.is_alive: return - # 精准计算正后方视角（无抖动关键） + # 精准计算正后方视角（核心优化：适配车辆z轴高度+坡度） vehicle_tf = self.vehicle.get_transform() + vehicle_loc = vehicle_tf.location # 获取车辆当前3D位置（包含z轴，适配上坡） yaw_rad = np.radians(vehicle_tf.rotation.yaw) - cam_x = vehicle_tf.location.x - (np.cos(yaw_rad) * self.view_distance) - cam_y = vehicle_tf.location.y - (np.sin(yaw_rad) * self.view_distance) - cam_z = self.view_height - # 强制设置视角（同步模式下无帧率波动，视角不抖） + # 1. 计算正后方水平偏移（x/y轴）→ 因view_distance增大，后移更明显 + cam_x = vehicle_loc.x - (np.cos(yaw_rad) * self.view_distance) + cam_y = vehicle_loc.y - (np.sin(yaw_rad) * self.view_distance) + + # 2. 计算z轴高度（核心优化：基于车辆当前z轴+基础高度+补偿） + cam_z = vehicle_loc.z + self.view_height + self.z_offset + + # 3. 强制设置视角（同步模式不变） spectator.set_transform(carla.Transform( carla.Location(x=cam_x, y=cam_y, z=cam_z), carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0) )) def get_observation(self): - """获取观测数据（原配置）""" + """获取观测数据（完全保留）""" return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) def step(self, action): - """执行单步动作（原无抖动逻辑，仅修改终止条件）""" + """执行单步动作（完全保留）""" if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁，请先调用reset()") - # 平滑车辆控制（原参数，无修改） + # 平滑车辆控制（原参数） throttle = 0.0 steer = 0.0 if action == 0: # 前进 @@ -250,19 +277,19 @@ def step(self, action): )) self.world.tick() # 同步帧推进（30fps，无帧率波动） - self.follow_vehicle() # 视角同步更新（无抖动） + self.follow_vehicle() # 调用优化后的视角跟随 next_state = self.get_observation() reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) - # ========== 仅修改：终止条件（撞车才结束，撞人/其他碰撞不结束） ========== + # 终止条件（撞车才结束） done = self.hit_vehicle return next_state, reward, done, {} def close(self): - """安全关闭环境（原配置+容错，仅新增清理NPC）""" - # ========== 仅新增：清理NPC ========== + """安全关闭环境（完全保留）""" + # 清理NPC for v in self.npc_vehicle_list: if v.is_alive: v.destroy() @@ -271,7 +298,7 @@ def close(self): p.destroy() self.traffic_manager.set_synchronous_mode(False) - # 原有清理逻辑（完全保留） + # 原有清理逻辑 try: self.sync_settings.synchronous_mode = False self.world.apply_settings(self.sync_settings) @@ -299,4 +326,3 @@ def close(self): time.sleep(0.5) print("✅ CARLA环境已关闭（同步模式已恢复为异步）") - From dd686124170f084f0fac7bd02e5332b4f5565d08 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Sun, 14 Dec 2025 14:29:48 +0800 Subject: [PATCH 18/18] updata --- src/MMAP-DRL-Nav/carla_environment.py | 86 +++++++++++++++++++-------- 1 file changed, 61 insertions(+), 25 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index 9326ed3f37..30c6cffe52 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -58,6 +58,10 @@ def is_port_used(port): self.hit_vehicle = False # 撞车标记（终止） self.hit_pedestrian = False # 撞人标记（不终止） + # 新增：出生点碰撞检测配置 + self.spawn_retry_times = 20 # 出生点重试次数 + self.spawn_safe_radius = 2.0 # 安全半径（避免近距离碰撞） + # 动作/观测空间 self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box( @@ -77,7 +81,40 @@ def is_port_used(port): self.view_distance = 8.0 # 正后方距离从6→8米（大幅后移，必看车屁股） self.z_offset = 0.5 # z轴补偿保留，避免上坡卡地下 - # ========== NPC生成逻辑（完全保留） ========== + # ========== 新增：安全生成车辆（解决碰撞问题） ========== + def _spawn_vehicle_safely(self, vehicle_bp): + """安全生成主角车辆，避免出生点碰撞""" + # 1. 获取所有可用出生点 + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + # 无预设出生点则生成随机位置 + spawn_points = [carla.Transform( + carla.Location(x=random.uniform(-50, 50), y=random.uniform(-50, 50), z=0.5), + carla.Rotation(yaw=random.uniform(0, 360)) + )] + + # 2. 重试生成车辆，直到成功或达到最大次数 + for attempt in range(self.spawn_retry_times): + # 随机选一个出生点 + spawn_point = random.choice(spawn_points) + + # 微调出生点高度（避免地面碰撞） + spawn_point.location.z += 0.2 + + try: + # 检测该位置是否有碰撞 + vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if vehicle is not None: + print(f"✅ 车辆生成成功（重试{attempt}次）") + return vehicle + except RuntimeError as e: + print(f"⚠️ 出生点碰撞，重试第{attempt+1}次...") + continue + + # 所有重试失败，抛出异常 + raise RuntimeError("❌ 所有出生点都有碰撞，无法生成车辆！") + + # ========== 优化NPC生成（避免与主角车辆碰撞） ========== def _spawn_small_npc(self): # 清理旧NPC for v in self.npc_vehicle_list: @@ -112,20 +149,31 @@ def _spawn_small_npc(self): else: spawn_points = spawn_points[:50] - # 生成50辆NPC车辆 + # 生成50辆NPC车辆（增加碰撞检测） for sp in spawn_points: try: - npc_vehicle = self.world.spawn_actor(random.choice(vehicle_bps), sp) - self.npc_vehicle_list.append(npc_vehicle) - npc_vehicle.set_autopilot(True, self.traffic_manager.get_port()) + # 微调NPC出生点高度，避免碰撞 + sp.location.z += 0.1 + npc_vehicle = self.world.try_spawn_actor(random.choice(vehicle_bps), sp) + if npc_vehicle is not None: + self.npc_vehicle_list.append(npc_vehicle) + npc_vehicle.set_autopilot(True, self.traffic_manager.get_port()) except: continue - # 生成5个NPC行人（数量不变） + # 生成5个NPC行人（数量不变，增加碰撞检测） pedestrian_bps = self.blueprint_library.filter('walker.pedestrian.*') for _ in range(5): - # 随机生成行人位置（地图内合法范围） - loc = carla.Location(x=random.uniform(-100, 100), y=random.uniform(-100, 100), z=0) + # 随机生成行人位置（远离主角车辆） + if self.vehicle is not None: + hero_loc = self.vehicle.get_transform().location + random_x = hero_loc.x + random.uniform(20, 50) * (1 if random.random()>0.5 else -1) + random_y = hero_loc.y + random.uniform(20, 50) * (1 if random.random()>0.5 else -1) + else: + random_x = np.random.uniform(-100, 100) + random_y = np.random.uniform(-100, 100) + + loc = carla.Location(x=random_x, y=random_y, z=0.1) try: pedestrian = self.world.try_spawn_actor(random.choice(pedestrian_bps), carla.Transform(loc)) if pedestrian: @@ -155,7 +203,7 @@ def _init_camera(self): self.camera.listen(lambda img: self._camera_callback(img)) def reset(self): - """重置环境（完全保留）""" + """重置环境（修复车辆生成碰撞问题）""" # 清理旧资源 if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() @@ -172,26 +220,15 @@ def reset(self): # 仅随机选择出生点（无任何打印） vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] - spawn_points = self.world.get_map().get_spawn_points() - # 随机选预设出生点（优先），无则随机生成坐标 - if len(spawn_points) > 0: - spawn_point = np.random.choice(spawn_points) - else: - # 随机生成合法范围坐标 - random_x = np.random.uniform(-200, 200) - random_y = np.random.uniform(-200, 200) - spawn_point = carla.Transform(carla.Location(x=random_x, y=random_y, z=0.5)) - - # 生成车辆 - self.vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) - self.vehicle.set_autopilot(False) + # 核心修复：使用安全生成方法替代直接生成 + self.vehicle = self._spawn_vehicle_safely(vehicle_bp) # 初始化传感器（调用补全的方法） self._init_camera() self._init_collision_sensor() - # 调用生成NPC + # 调用生成NPC（已优化碰撞） self._spawn_small_npc() # 等待传感器就绪 @@ -324,5 +361,4 @@ def close(self): print(f"⚠️ 销毁碰撞传感器时警告：{e}") time.sleep(0.5) - print("✅ CARLA环境已关闭（同步模式已恢复为异步）") - + print("✅ CARLA环境已关闭（同步模式已恢复为异步）") \ No newline at end of file