From 3b871c61c5e74a3cfa94193c7fdb0595744d0d42 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Mon, 22 Sep 2025 10:29:34 +0800
Subject: [PATCH 01/19] =?UTF-8?q?=E4=BD=BF=E7=94=A8DQN=E5=92=8CPPO?=
 =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E8=87=AA=E5=8A=A8=E9=A9=BE=E9=A9=B6=E6=B1=BD?=
 =?UTF-8?q?=E8=BD=A6=E5=AF=BC=E8=88=AA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/self_driving_car_navigation/README.md | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 src/self_driving_car_navigation/README.md

diff --git a/src/self_driving_car_navigation/README.md b/src/self_driving_car_navigation/README.md
new file mode 100644
index 0000000000..e69de29bb2

From a12a0f690f1820e26664247da03748f5f751a600 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Fri, 24 Oct 2025 23:44:41 +0800
Subject: [PATCH 02/19] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=88=91=E7=9A=84?=
 =?UTF-8?q?=E4=B8=BB=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/self_driving_car_navigation/main.py | 82 +++++++++++++++++++++++++
 1 file changed, 82 insertions(+)
 create mode 100644 src/self_driving_car_navigation/main.py

diff --git a/src/self_driving_car_navigation/main.py b/src/self_driving_car_navigation/main.py
new file mode 100644
index 0000000000..4cada34754
--- /dev/null
+++ b/src/self_driving_car_navigation/main.py
@@ -0,0 +1,82 @@
+import torch
+import torch.nn as nn
+import argparse 
+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(nn.Module):
+    
+    def __init__(self, device='cpu', state_dim=128, action_dim=2):
+        super().__init__()
+        self.device = device
+        self.perception = PerceptionModule().to(self.device)
+        self.attention = CrossDomainAttention(num_blocks=2,embed_dim=64).to(self.device)
+        self.decision = DecisionModule().to(self.device)
+        self.sagm = SelfAssessmentGradientModel(hidden_dim=64 ).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)
+        
+        action_3d = action.unsqueeze(1)
+        seq_len = fused_features.shape[1]
+        action_3d = action_3d.repeat(1, seq_len, 1)
+
+        sagm_q_value = self.sagm(fused_features, action_3d)  
+        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}')
+import argparse
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--mode", type=str, default="train", choices=["train", "test"])
+    args = parser.parse_args()
+
+    device = "cuda" if torch.cuda.is_available() else "cpu"
+    model = IntegratedSystem(device=device)
+    optimizer = Adam(model.parameters(), lr=0.001)
+    dataloader = get_dataloader()
+
+    if args.mode == "train":
+        train_model(model, dataloader, optimizer, device)
+    elif args.mode == "test":
+        test_model(model, dataloader, device)
\ No newline at end of file

From 212bda52cbe3a846c9cdd23023be175f1bcfd23a Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Sun, 26 Oct 2025 11:02:51 +0800
Subject: [PATCH 03/19] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E4=B8=BB=E5=87=BD?=
 =?UTF-8?q?=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../attention_module.py                       | 56 ++++++++++++
 src/self_driving_car_navigation/dataloader.py | 22 +++++
 .../decision_module.py                        | 26 ++++++
 src/self_driving_car_navigation/dqn_agent.py  | 87 +++++++++++++++++++
 .../perception_module.py                      | 26 ++++++
 src/self_driving_car_navigation/sagm.py       | 87 +++++++++++++++++++
 6 files changed, 304 insertions(+)
 create mode 100644 src/self_driving_car_navigation/attention_module.py
 create mode 100644 src/self_driving_car_navigation/dataloader.py
 create mode 100644 src/self_driving_car_navigation/decision_module.py
 create mode 100644 src/self_driving_car_navigation/dqn_agent.py
 create mode 100644 src/self_driving_car_navigation/perception_module.py
 create mode 100644 src/self_driving_car_navigation/sagm.py

diff --git a/src/self_driving_car_navigation/attention_module.py b/src/self_driving_car_navigation/attention_module.py
new file mode 100644
index 0000000000..8faa2bf93e
--- /dev/null
+++ b/src/self_driving_car_navigation/attention_module.py
@@ -0,0 +1,56 @@
+import torch
+import torch.nn as nn
+from torch.nn import MultiheadAttention
+
+class CrossDomainAttention(nn.Module):
+    def __init__(self, num_blocks=2, embed_dim=64):  # 关键修改：将embed_dim改为256（与输入特征维度匹配）
+        super().__init__()
+        self.num_blocks = num_blocks
+        # 多头注意力层的embed_dim必须与输入特征的最后一维一致
+        self.attention_blocks = nn.ModuleList([
+            MultiheadAttention(embed_dim=embed_dim, num_heads=2, batch_first=True)  # 使用传入的embed_dim
+            for _ in range(num_blocks)
+        ])
+        self.norm = nn.LayerNorm(embed_dim)  # 层归一化的维度也需匹配
+
+    def forward(self, scene_info, segmentation, odometry, obstacles, boundary):
+        # 统一输入张量维度为3D [batch, seq_len, features]
+        def adjust_dim(tensor):
+            if tensor.dim() == 4:
+                # 4D张量（如[batch, channel, h, w]）→ [batch, h*w, channel]
+                batch, channel, h, w = tensor.shape
+                return tensor.permute(0, 2, 3, 1).reshape(batch, h*w, channel)
+            elif tensor.dim() == 2:
+                # 2D张量（如[batch, features]）→ [batch, 1, features]
+                return tensor.unsqueeze(1)
+            else:
+                return tensor
+
+        # 调整所有输入特征的维度
+        inputs = [
+            adjust_dim(scene_info),
+            adjust_dim(segmentation),
+            adjust_dim(odometry),
+            adjust_dim(obstacles),
+            adjust_dim(boundary)
+        ]
+
+        # 统一所有特征的最后一维（特征维度）为 embed_dim（256）
+        target_feat_dim = self.attention_blocks[0].embed_dim  # 从注意力模块获取目标维度（256）
+        adjusted_inputs = []
+        for x in inputs:
+            if x.shape[-1] != target_feat_dim:
+                # 用线性层将特征维度转换为目标维度（256）
+                linear = nn.Linear(x.shape[-1], target_feat_dim, device=x.device)
+                x = linear(x)
+            adjusted_inputs.append(x)
+
+        # 拼接所有特征（在seq_len维度拼接）
+        x = torch.cat(adjusted_inputs, dim=1)  # 此时x的形状为 [batch, total_seq_len, 256]
+
+        # 注意力计算（此时输入维度与embed_dim匹配）
+        for attn_block in self.attention_blocks:
+            attn_output, _ = attn_block(x, x, x)  # 自注意力计算
+            x = self.norm(x + attn_output)  # 残差连接 + 层归一化
+
+        return x
\ No newline at end of file
diff --git a/src/self_driving_car_navigation/dataloader.py b/src/self_driving_car_navigation/dataloader.py
new file mode 100644
index 0000000000..7b0de1119b
--- /dev/null
+++ b/src/self_driving_car_navigation/dataloader.py
@@ -0,0 +1,22 @@
+import torch
+from torch.utils.data import Dataset, DataLoader
+
+class CarlaDataset(Dataset):
+    def __init__(self, num_samples=1000):
+        self.num_samples = num_samples
+    
+    def __len__(self):
+        return self.num_samples
+    
+    def __getitem__(self, idx):
+        image = torch.randn(3, 64, 64)  # Example image data
+        lidar_data = torch.randn(1, 64, 64)  # Example LiDAR data
+        imu_data = torch.randn(6)  # Example IMU data
+        action = torch.randn(2)  # Example action data
+
+        return image, lidar_data, imu_data, action
+
+def get_dataloader(batch_size=2, num_samples=200):
+    dataset = CarlaDataset(num_samples=num_samples)
+    dataloader = DataLoader(dataset, batch_size=batch_size, shuffle=True,num_workers=0)
+    return dataloader
diff --git a/src/self_driving_car_navigation/decision_module.py b/src/self_driving_car_navigation/decision_module.py
new file mode 100644
index 0000000000..147d9664e0
--- /dev/null
+++ b/src/self_driving_car_navigation/decision_module.py
@@ -0,0 +1,26 @@
+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(64, 128),
+            nn.ReLU(),
+            nn.Linear(128, 64),
+            nn.ReLU(),
+            nn.Linear(64, 2)  # 输出：转向角和油门
+        )
+        self.value_net = nn.Sequential(
+            nn.Linear(64, 128),
+            nn.ReLU(),
+            nn.Linear(128, 64),
+            nn.ReLU(),
+            nn.Linear(64, 1)  # 值函数
+        )
+
+    def forward(self, fused_features):
+        aggregated_features = fused_features.mean(dim=1)
+        policy = self.policy_net(fused_features)
+        value = self.value_net(fused_features)
+        return policy, value
diff --git a/src/self_driving_car_navigation/dqn_agent.py b/src/self_driving_car_navigation/dqn_agent.py
new file mode 100644
index 0000000000..2ef914b102
--- /dev/null
+++ b/src/self_driving_car_navigation/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
diff --git a/src/self_driving_car_navigation/perception_module.py b/src/self_driving_car_navigation/perception_module.py
new file mode 100644
index 0000000000..7f06d236d1
--- /dev/null
+++ b/src/self_driving_car_navigation/perception_module.py
@@ -0,0 +1,26 @@
+import torch
+import torch.nn as nn
+
+class PerceptionModule(nn.Module):
+    def __init__(self):
+        super(PerceptionModule, self).__init__()
+        self.segmentation_net = nn.Sequential(
+            nn.Conv2d(3, 16, kernel_size=3, stride=1, padding=1),
+            nn.ReLU(),
+            nn.MaxPool2d(kernel_size=2, stride=2),
+            nn.Conv2d(16, 128, kernel_size=3, stride=1, padding=1),
+            nn.ReLU(),
+            nn.MaxPool2d(kernel_size=2, stride=2),
+            nn.Conv2d(128, 256, kernel_size=3, stride=1, padding=1),
+            nn.ReLU(),
+            nn.MaxPool2d(kernel_size=2, stride=2)
+        )
+
+    def forward(self, imu_data, image, lidar_data):
+        segmentation = self.segmentation_net(image)
+        scene_info = segmentation.mean(dim=(2, 3))  # 视觉信息提取
+        odometry = imu_data  # IMU用于里程计
+        obstacles = lidar_data.mean(dim=1)  # 障碍物检测
+        boundary = lidar_data.max(dim=1)[0]  # 边界检测
+
+        return scene_info, segmentation, odometry, obstacles, boundary
diff --git a/src/self_driving_car_navigation/sagm.py b/src/self_driving_car_navigation/sagm.py
new file mode 100644
index 0000000000..7f9b194fa7
--- /dev/null
+++ b/src/self_driving_car_navigation/sagm.py
@@ -0,0 +1,87 @@
+import torch
+import torch.nn as nn
+import torch.optim as optim
+import torch.nn.functional as F
+import logging
+
+class SelfAssessmentGradientModel(nn.Module):
+    def __init__(self, hidden_dim=64, output_dim=1):
+        super(SelfAssessmentGradientModel, self).__init__()
+        self.fc1 = nn.Linear(66, hidden_dim)
+        self.fc2 = nn.Linear(hidden_dim, output_dim)
+        self.activation = nn.ReLU()
+
+    def forward(self, state, action):
+        x = torch.cat((state, action), dim=-1)  
+        x = x.reshape(-1, 66)
+        x = self.activation(self.fc1(x))
+        q_value = self.fc2(x)
+        q_value = q_value.view(state.size(0), state.size(1), -1)
+        return q_value
+if __name__ == "__main__":
+    state_dim = 10  
+    action_dim = 2  
+    input_dim = state_dim + action_dim
+    hidden_dim = 64
+    output_dim = 1
+    discount_factor = 0.99
+    weight_sagm = 0.5  
+
+    sagm = SelfAssessmentGradientModel(input_dim=input_dim, hidden_dim=hidden_dim, output_dim=output_dim)
+    critic = nn.Linear(input_dim, 1)  
+    actor = nn.Linear(state_dim, action_dim)  
+
+    optimizer_sagm = optim.Adam(sagm.parameters(), lr=0.0001)
+    optimizer_critic = optim.Adam(critic.parameters(), lr=0.0001)
+    optimizer_actor = optim.Adam(actor.parameters(), lr=0.0001)
+
+    logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(message)s')
+    log_interval = 10  
+
+    def update_sagm(sagm, optimizer_sagm, state, action, target_q_value):
+        sagm_q_value = sagm(state, action)
+        sagm_loss = F.mse_loss(sagm_q_value, target_q_value)
+        optimizer_sagm.zero_grad()
+        sagm_loss.backward(retain_graph=True)
+        optimizer_sagm.step()
+        return sagm_q_value
+
+    def compute_target_q_value(reward, next_state, critic, actor):
+        with torch.no_grad():
+            next_action = actor(next_state)
+            target_q_value = reward + discount_factor * critic(torch.cat((next_state, next_action), dim=-1))
+        return target_q_value
+
+    def update_actor_critic(critic, actor, sagm_q_value, state, action):
+        total_q_value = (1 - weight_sagm) * critic(torch.cat((state, action), dim=-1)) + weight_sagm * sagm_q_value
+        total_loss = -total_q_value.mean()  
+        
+        optimizer_actor.zero_grad()
+        optimizer_critic.zero_grad()
+        total_loss.backward()
+        optimizer_actor.step()
+        optimizer_critic.step()
+
+    total_episodes = 1000  
+    for episode in range(total_episodes):
+        state = torch.randn(1, state_dim)  
+        done = False
+        episode_reward = 0
+
+        while not done:
+            action = actor(state)
+            next_state = torch.randn(1, state_dim)  
+            reward = torch.tensor([[1.0]])  
+            done = torch.rand(1).item() > 0.95  
+            episode_reward += reward.item()
+            
+            target_q_value = compute_target_q_value(reward, next_state, critic, actor)
+            
+            sagm_q_value = update_sagm(sagm, optimizer_sagm, state, action, target_q_value)
+            
+            update_actor_critic(critic, actor, sagm_q_value, state, action)
+            
+            state = next_state
+
+        if episode % log_interval == 0:
+            logging.info(f'Episode: {episode}, Reward: {episode_reward:.2f}')

From 6f77ded0d7cb2a330c9a33489c8ef1fa647b9a9b Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Tue, 11 Nov 2025 15:31:07 +0800
Subject: [PATCH 04/19] =?UTF-8?q?=E5=86=99=E5=A5=BD=E4=BA=86=E6=88=91?=
 =?UTF-8?q?=E7=9A=84REANME?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/self_driving_car_navigation/README.md | 135 ++++++++++++++++++++++
 1 file changed, 135 insertions(+)

diff --git a/src/self_driving_car_navigation/README.md b/src/self_driving_car_navigation/README.md
index e69de29bb2..8fb3467c36 100644
--- a/src/self_driving_car_navigation/README.md
+++ b/src/self_driving_car_navigation/README.md
@@ -0,0 +1,135 @@
+# 自动驾驶仿真学习环境（基于Carla与Python3.7）
+
+## 项目概述
+本项目构建了基于Carla 0.9.11仿真平台和Python 3.7的自动驾驶学习环境，适用于车辆控制、场景感知、路径规划等自动驾驶相关算法的学习与实践。通过VSCode开发环境实现代码编写、调试与运行的一体化流程，支持从基础车辆控制到复杂场景仿真的完整学习路径。
+
+## 环境准备
+
+### 依赖库安装
+```bash
+# 安装Python 3.7（建议使用虚拟环境）
+# 安装Carla 0.9.11客户端
+pip install carla==0.9.11
+# 安装辅助依赖库
+pip install numpy opencv-python matplotlib vscode-debugpy
+```
+- `carla==0.9.11`：自动驾驶仿真平台核心库，提供车辆、环境与传感器模拟
+- `python 3.7`：项目开发与运行的Python版本
+- `numpy`：数值计算支持
+- `opencv-python`：图像数据处理
+- `matplotlib`：数据可视化工具
+- `vscode-debugpy`：VSCode调试支持
+
+### 开发环境配置
+1. 下载并安装[Carla 0.9.11官方发行版](https://github.com/carla-simulator/carla/releases/tag/0.9.11)
+2. 安装[VSCode](https://code.visualstudio.com/)并配置Python 3.7解释器
+3. 推荐插件：Python、Pylance、Code Runner（提升开发效率）
+
+## 项目结构
+
+| 文件名             | 功能描述                                                     |
+|-------------------|--------------------------------------------------------------|
+| `main.py`         | 核心程序入口，实现Carla客户端连接、世界初始化与主循环控制       |
+| `vehicle_control.py`| 车辆控制模块，实现油门、刹车、转向等基础控制逻辑               |
+| `scene_generation.py`| 场景生成工具，支持随机障碍物、天气变化与交通参与者生成         |
+| `sensor_manager.py`| 传感器管理模块，处理摄像头、激光雷达等数据采集与解析           |
+| `utils.py`        | 通用工具函数，包含坐标转换、数据可视化等辅助功能               |
+| `config.yaml`     | 配置文件，存储仿真参数（如帧率、传感器类型、车辆模型等）       |
+| `README.md`       | 项目说明文档                                                   |
+
+## 核心功能
+
+### 1. 基础车辆控制（main.py & vehicle_control.py）
+- 客户端连接管理：自动连接Carla服务器，支持断开重连机制
+- 多车辆控制：同时控制多辆自动驾驶车辆，实现编队行驶模拟
+- 控制模式切换：支持手动控制（键盘）与自动控制（程序）模式切换
+- 状态实时反馈：在VSCode终端输出车辆速度、位置等关键信息
+
+```python
+# 车辆控制逻辑示例
+import carla
+
+# 连接到Carla服务器
+client = carla.Client('localhost', 2000)
+client.set_timeout(10.0)
+world = client.get_world()
+
+# 获取车辆控制器
+vehicle = world.get_actors().filter('*vehicle*')[0]
+control = carla.VehicleControl()
+
+# 设定前进指令（油门0.5，转向0）
+control.throttle = 0.5
+control.steer = 0.0
+vehicle.apply_control(control)
+```
+
+### 2. 场景仿真与传感器（scene_generation.py & sensor_manager.py）
+- 动态场景生成：支持随机天气（雨、雾、时间）、障碍物与交通灯配置
+- 多传感器集成：摄像头（RGB/深度）、激光雷达、毫米波雷达数据采集
+- 数据同步存储：传感器数据与车辆状态时间戳同步，便于离线分析
+- VSCode调试支持：断点调试传感器数据处理流程，直观查看数据格式
+
+```python
+# 传感器配置示例
+def setup_camera(world, vehicle):
+    camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')
+    camera_bp.set_attribute('image_size_x', '1280')
+    camera_bp.set_attribute('image_size_y', '720')
+    # 安装在车辆前方
+    transform = carla.Transform(carla.Location(x=1.5, z=2.4))
+    camera = world.spawn_actor(camera_bp, transform, attach_to=vehicle)
+    # 定义数据回调函数
+    camera.listen(lambda image: process_image(image))
+    return camera
+```
+
+### 3. 学习任务支持
+- 路径跟踪练习：预设参考路径，实现PID等控制算法跟踪
+- 避障场景训练：生成动态障碍物，练习碰撞检测与规避逻辑
+- 数据采集工具：批量采集不同场景下的传感器数据，用于模型训练
+
+## 使用方法
+
+1. 启动Carla服务器：
+```bash
+# 在Carla安装目录下执行
+./CarlaUE4.sh  # Linux/Mac
+CarlaUE4.exe   # Windows
+```
+
+2. 基础车辆控制示例：
+```bash
+python main.py --mode manual  # 手动控制模式
+python main.py --mode auto    # 自动控制模式
+```
+
+3. 场景仿真运行：
+```bash
+python scene_generation.py --weather rain --obstacles 5
+```
+
+4. 传感器数据采集：
+```bash
+python sensor_manager.py --record --output ./data
+```
+
+### VSCode开发提示
+- 按`F5`启动调试模式（需配置`.vscode/launch.json`）
+- 使用Code Runner插件（右键`Run Code`）快速执行单文件
+- 推荐使用VSCode的Jupyter插件进行分步调试与数据可视化
+
+## 参数调整指南
+
+| 参数               | 调整范围 | 效果说明                         |
+|-------------------|----------|----------------------------------|
+| `throttle_gain`   | 0.1~1.0 | 增大会提高加速响应，过大会导致打滑 |
+| `sensor_fps`      | 10~60   | 提高值增加数据精度（增加计算量）  |
+| `obstacle_density`| 0~20    | 增大会增加场景复杂度             |
+| `simulation_delta_seconds`| 0.01~0.1 | 减小值提高仿真精度（降低运行速度）|
+
+## 参考资料
+- [Carla 0.9.11官方文档](https://carla.readthedocs.io/en/0.9.11/)
+- [Python 3.7官方文档](https://docs.python.org/3.7/)
+- [VSCode Python开发指南](https://code.visualstudio.com/docs/languages/python)
+- [Carla自动驾驶教程](https://carla.readthedocs.io/en/latest/tutorials/)
\ No newline at end of file

From f9113fd9d289c3c8eebab5bdc19160dadd7c1789 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Mon, 17 Nov 2025 14:35:00 +0800
Subject: [PATCH 05/19] =?UTF-8?q?=E5=AE=8C=E6=88=90=E4=BA=86=E6=9C=AC?=
 =?UTF-8?q?=E6=AC=A1=E4=BD=9C=E4=B8=9A=EF=BC=8C=E4=BF=AE=E6=94=B9=E4=BA=86?=
 =?UTF-8?q?=E4=BA=9B=E8=AE=B8=E4=BB=A3=E7=A0=81=E4=B8=AD=E7=9A=84=E9=94=99?=
 =?UTF-8?q?=E8=AF=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../RL_QG_agent.py                            | 193 +++---
 src/chap14_reinforcement_learning/__init__.py |   4 +-
 .../reversi/__init__.py                       |   8 +
 .../reversi/reversi.py                        | 583 ++++++++----------
 .../reversi_main.py                           | 178 +++---
 5 files changed, 411 insertions(+), 555 deletions(-)

diff --git a/src/chap14_reinforcement_learning/RL_QG_agent.py b/src/chap14_reinforcement_learning/RL_QG_agent.py
index fa000cafcf..588fc5a0ef 100644
--- a/src/chap14_reinforcement_learning/RL_QG_agent.py
+++ b/src/chap14_reinforcement_learning/RL_QG_agent.py
@@ -1,139 +1,112 @@
-# 导入必要的库
-import os           # 导入操作系统接口，用于文件路径处理和目录操作
-import numpy as np  # 导入数值计算库，用于数组操作和数学计算
-import tensorflow as tf  # 导入深度学习框架，用于构建和训练神经网络
+
+import os
+import numpy as np
+import tensorflow as tf                 #要用到1.x的版本，我电脑上是2.x的版本
+tf.compat.v1.disable_eager_execution()  # 禁用即时执行，兼容1.x语法
 
 class RL_QG_agent:
-    """黑白棋强化学习智能体，基于Q学习和卷积神经网络实现落子策略"""
+    """黑白棋强化学习智能体，基于Q学习和卷积神经网络"""
     
     def __init__(self):
-        """初始化智能体，设置模型保存路径和TensorFlow相关组件"""
-        # 确定模型保存目录：当前脚本所在目录下的Reversi文件夹
+        """初始化智能体参数和模型路径"""
         self.model_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "Reversi")
-        os.makedirs(self.model_dir, exist_ok=True)  # 创建目录（若不存在）
+        os.makedirs(self.model_dir, exist_ok=True)  # 创建模型保存目录
         
-        # TensorFlow相关组件占位符
-        self.sess = None          # 会话对象，管理TensorFlow图的执行
-        self.saver = None         # 模型保存器，用于保存和加载参数
-        self.input_states = None  # 网络输入张量（棋盘状态）
-        self.Q_values = None      # 网络输出张量（各位置Q值）
-
+        # TensorFlow组件
+        self.sess = None          # 会话
+        self.saver = None         # 模型保存器
+        self.input_states = None  # 输入张量
+        self.Q_values = None      # Q值输出张量
 
     def init_model(self):
-        """
-        构建并初始化卷积神经网络模型
+        """构建卷积神经网络模型"""
+        self.sess = tf.compat.v1.Session()
         
-        网络结构:
-            1. 卷积层1: 32个3x3卷积核(提取局部棋子模式)
-            2. 卷积层2: 64个3x3卷积核(提取全局布局特征)
-            3. 扁平化层: 将特征图转换为一维向量
-            4. 全连接层: 512个神经元(学习特征组合)
-            5. 输出层: 64个神经元(对应棋盘各位置Q值)
-        """
-        self.sess = tf.Session()  # 创建TensorFlow会话
+        # 输入：[批次大小, 8, 8, 3]（3通道：黑棋、白棋、当前玩家）
+        self.input_states = tf.compat.v1.placeholder(
+            tf.float32, shape=[None, 8, 8, 3], name="input_states"
+        )
         
-        # 定义网络输入：[批次大小, 棋盘高度, 棋盘宽度, 通道数]
-        self.input_states = tf.placeholder(
-            tf.float32,             # 输入数据类型为32位浮点数
-            shape=[None, 8, 8, 3],  # 输入张量的形状
-            name="input_states"     # 该张量在计算图中的名称
+        # 卷积层1：32个3x3卷积核
+        conv1 = tf.compat.v1.layers.conv2d(
+            inputs=self.input_states,
+            filters=32,
+            kernel_size=3,
+            padding="same",
+            activation=tf.nn.relu
         )
         
-        # ========== 卷积层1：提取局部棋子模式特征 ==========
-        # 32个3x3卷积核，捕捉相邻棋子的局部关系
-        # 输出形状：[None, 8, 8, 32]
-        conv1 = tf.layers.conv2d(
-            # 输入数据：状态特征图
-            inputs = self.input_states,
-            filters = 32,            # 32个卷积核，生成32个特征图
-            kernel_size = 3,         # 3x3卷积核，捕捉局部区域
-            padding = "same",        # 同尺寸填充，保持输出尺寸与输入一致
-            activation = tf.nn.relu  # ReLU激活函数，引入非线性
+        # 卷积层2：64个3x3卷积核
+        conv2 = tf.compat.v1.layers.conv2d(
+            inputs=conv1,
+            filters=64,
+            kernel_size=3,
+            padding="same",
+            activation=tf.nn.relu
         )
-
-        # ========== 卷积层2：提取全局布局特征 ==========
-        # 64个3x3卷积核，捕捉更复杂的棋子布局模式
-        # 输出形状：[None, 8, 8, 64]
-        conv2 = tf.layers.conv2d(
-            inputs = conv1,
-            filters = 64,            # 特征图数量翻倍，增强特征表达能力
-            kernel_size = 3,         # 使用3×3的卷积核，平衡特征提取能力与参数量
-            padding = "same",        # 保持输出特征图尺寸与输入一致（补零填充）
-            activation = tf.nn.relu  # ReLU激活函数，引入非线性并抑制负梯
+        
+        # 扁平化
+        flat = tf.compat.v1.layers.flatten(conv2)
+        
+        # 全连接层
+        dense = tf.compat.v1.layers.dense(
+            inputs=flat,
+            units=512,
+            activation=tf.nn.relu
         )
-
-        # ========== 扁平化层：将多维特征转为一维向量 ==========
-        # 输入形状[None, 8, 8, 64] → 输出形状[None, 4096]
-        flat = tf.layers.flatten(conv2)
-
-        # ========== 全连接层：学习特征间的全局关系 ==========
-        # 512个神经元，通过ReLU激活学习非线性组合
-        # 输出形状：[None, 512]
-        dense = tf.layers.dense(inputs = flat, units = 512, activation = tf.nn.relu)
-
-        # ========== 输出层：预测各位置的Q值 ==========
-        # 64个神经元对应棋盘64个位置，直接输出Q值（无激活函数）
-        self.Q_values = tf.layers.dense(inputs = dense, units = 64, name = "q_values")
-
-        # 初始化所有变量并创建模型保存器
-        self.sess.run(tf.global_variables_initializer())
-        self.saver = tf.train.Saver()
-
+        
+        # 输出层：64个Q值（对应8x8棋盘）
+        self.Q_values = tf.compat.v1.layers.dense(
+            inputs=dense,
+            units=64,
+            name="q_values"
+        )
+        
+        # 初始化变量和保存器
+        self.sess.run(tf.compat.v1.global_variables_initializer())
+        self.saver = tf.compat.v1.train.Saver()
 
     def place(self, state, enables):
-        """
-        根据当前棋盘状态和合法落子位置，选择最优落子位置
-        
-        :param state: 当前棋盘状态，形状为(8, 8, 3)的NumPy数组
-                      3个通道分别表示：黑棋位置、白棋位置、当前玩家
-        :param enables: 合法落子位置的索引列表（0-63）
-        :return: 选择的落子位置索引（0-63）
-        """
-        # 状态预处理：转换为适合网络输入的形状 [1, 8, 8, 3]
+        """根据当前状态和合法动作选择最优落子位置"""
+        # 状态预处理：[1, 8, 8, 3]
         state_input = np.array(state).reshape(1, 8, 8, 3).astype(np.float32)
         
-        # 前向传播获取所有位置的Q值
+        # 计算所有位置的Q值
         q_vals = self.sess.run(self.Q_values, feed_dict={self.input_states: state_input})
         
-        # 提取合法位置的Q值
-        legal_q = q_vals[0][enables]  # 形状与enables长度一致
+        # 提取合法动作的Q值
+        legal_q = q_vals[0][enables]
         
-        # 处理所有合法Q值为0的特殊情况（随机选择）
+        # 处理无有效Q值的情况（随机选择）
         if np.sum(legal_q) == 0:
-            return np.random.choice(enables)  # 从合法位置中随机选
+            return np.random.choice(enables)
         
-        # 选择Q值最大的位置（若有多个，随机选一个）
-        max_q = np.max(legal_q)                # 最大Q值
-        best_indices = np.where(legal_q == max_q)[0]  # 在合法动作中找出所有具有最大Q值的动作索引
-        return enables[np.random.choice(best_indices)]  # 映射回原始位置
-
+        # 选择Q值最大的动作（若有多个，随机选一个）
+        max_q = np.max(legal_q)
+        best_indices = np.where(legal_q == max_q)[0]
+        return enables[np.random.choice(best_indices)]
 
     def save_model(self):
-        """保存训练好的模型参数到指定目录"""
+        """保存模型参数"""
+        try:
+            model_path = os.path.join(self.model_dir, 'parameter.ckpt')
+            self.saver.save(self.sess, model_path)
+            print("模型已保存至", self.model_dir)
+        except Exception as e:
+            print("保存模型时出错:", e)
 
-        # 使用 TensorFlow 的 Saver 对象保存模型参数到指定路径
-        # self.sess 是当前的 TensorFlow 会话对象，用于执行计算图中的操作
-        # os.path.join(self.model_dir, 'parameter.ckpt') 用于构建保存模型参数的完整文件路径
-        # self.model_dir 是保存模型的目录路径
-        # 'parameter.ckpt' 是保存模型参数的文件名
-        self.saver.save(self.sess, os.path.join(self.model_dir, 'parameter.ckpt'))
-        print("模型已保存至", self.model_dir)
+    def load_model(self):
+        """加载模型参数"""
+        if self.sess is None:
+            self.init_model()  # 未初始化则先构建模型
         
-        # 尝试调用saver的save方法保存当前会话(sess)的模型参数
-        # model_path 指定了模型保存的路径和文件名
-        self.saver.save(self.sess, model_path)
+        model_path = os.path.join(self.model_dir, 'parameter.ckpt')
+        if not os.path.exists(model_path + '.index'):
+            print("模型文件不存在，使用初始化模型")
+            return
         
-        # 如果保存过程中发生任何异常，将会被捕获并处理
+        try:
+            self.saver.restore(self.sess, model_path)
+            print("模型已从", self.model_dir, "加载")
         except Exception as e:
-            self.logger.error("保存模型时出错: %s", e) # 使用logger记录一条错误日志信息
-
-
-
-    def load_model(self):
-        """从指定目录加载预训练的模型参数
-        异常处理:
-        检查模型文件是否存在
-        捕获并处理恢复会话时可能出现的错误
-        """
-        self.saver.restore(self.sess, os.path.join(self.model_dir, 'parameter.ckpt'))
-        print("模型已从", self.model_dir, "加载")
+            print("加载模型时出错:", e)
\ No newline at end of file
diff --git a/src/chap14_reinforcement_learning/__init__.py b/src/chap14_reinforcement_learning/__init__.py
index 1a7e24a9d8..e8305405fe 100644
--- a/src/chap14_reinforcement_learning/__init__.py
+++ b/src/chap14_reinforcement_learning/__init__.py
@@ -166,13 +166,13 @@
 # 1. 21点游戏：经典扑克牌游戏（考察决策策略）
 register(
     id='Blackjack-v0',
-    entry_point='gym.envs.toy_text:BlackjackEnv',
+    entry_point='gym.envs.reversi:ReversiEnv',
 )
 
 # 2. 凯利判赌任务：基于概率的赌博决策（考察期望收益计算）
 register(
     id='KellyCoinflip-v0',
-    entry_point='gym.envs.toy_text:KellyCoinflipEnv',
+    entry_point='gym.envs.reversi:ReversiEnv',
     reward_threshold=246.61,  # 理论最优收益阈值
 )
 
diff --git a/src/chap14_reinforcement_learning/reversi/__init__.py b/src/chap14_reinforcement_learning/reversi/__init__.py
index 1f99a92524..3ae20479ef 100644
--- a/src/chap14_reinforcement_learning/reversi/__init__.py
+++ b/src/chap14_reinforcement_learning/reversi/__init__.py
@@ -1 +1,9 @@
 from gym.envs.reversi.reversi import ReversiEnv # 从OpenAI Gym的Reversi（黑白棋）环境实现中导入核心环境类
+from gym.envs.registration import register  # 关键导入语句
+# 注册黑白棋环境（8x8棋盘）
+register(
+    id='Reversi8x8-v0',
+    entry_point='gym.envs.reversi:ReversiEnv',  # 确保reversi.py中存在ReversiEnv类
+    kwargs={'board_size': 8},
+    max_episode_steps=1000,
+)
\ No newline at end of file
diff --git a/src/chap14_reinforcement_learning/reversi/reversi.py b/src/chap14_reinforcement_learning/reversi/reversi.py
index c8efa49c0c..d41f30165b 100644
--- a/src/chap14_reinforcement_learning/reversi/reversi.py
+++ b/src/chap14_reinforcement_learning/reversi/reversi.py
@@ -1,7 +1,3 @@
-"""
-Game of Reversi
-"""
-
 from six import StringIO
 import sys
 import gym
@@ -9,384 +5,287 @@
 import numpy as np
 from gym import error
 from gym.utils import seeding
- #这段代码定义了一个随机策略函数 random_policy，用于在黑白棋（Reversi/Othello）游戏中为当前玩家随机选择一个合法的落子动作（包括“跳过”动作）
+
+# 随机策略函数：为当前玩家随机选择合法落子动作
 def make_random_policy(np_random):
     def random_policy(state, player_color):
         possible_places = ReversiEnv.get_possible_actions(state, player_color)
         # 没有可落子位置，返回"pass"动作
         if len(possible_places) == 0:
-            d = state.shape[-1]#动态获取棋盘的边长
-            return d**2 + 1    # pass动作
+            d = state.shape[-1]  # 动态获取棋盘的边长
+            return d**2 + 1     # pass动作
         # 随机选择一个可能的动作
-        a = np_random.randint(len(possible_places)) # 生成一个随机索引
-        return possible_places[a] # 返回对应索引的放置位置
-    return random_policy # 返回定义好的随机策略函数
+        a = np_random.randint(len(possible_places))  # 生成随机索引
+        return possible_places[a]  # 返回对应索引的放置位置
+    return random_policy  # 返回定义好的随机策略函数
 
 class ReversiEnv(gym.Env):
-    """
-    Reversi environment. Play against a fixed opponent.
-    """
-    BLACK = 0
-    WHITE = 1
-    metadata = {"render.modes": ["ansi","human"]}
-
-    def __init__(self, player_color, opponent, observation_type, illegal_place_mode, board_size):
-        """
-        参数:
-            player_color: 代理(玩家)的棋子颜色，'black'或'white'
-            opponent: 对手策略，可以是'random'或自定义策略函数
-            observation_type: 状态编码方式，目前仅支持'numpy3c'
-            illegal_place_mode: 处理非法落子的方式，'lose'(自动输)或'raise'(抛出异常)
-            board_size: 棋盘大小，默认8x8
-        """
-        assert isinstance(board_size, int) and board_size >= 1, 'Invalid board size: {}'.format(board_size)
+    """黑白棋环境，支持标准8x8棋盘和强化学习接口"""
+    BLACK = 0  # 黑棋内部标识
+    WHITE = 1  # 白棋内部标识
+    metadata = {"render.modes": ["ansi", "human"]}
+
+    def __init__(self, player_color, opponent, observation_type, illegal_place_mode, board_size=8):
+        """初始化环境参数"""
+        assert isinstance(board_size, int) and board_size >= 4, '棋盘大小必须≥4（偶数）'
         self.board_size = board_size
 
-        # 将颜色字符串映射为内部表示
-        colormap = {
-            'black': ReversiEnv.BLACK,
-            'white': ReversiEnv.WHITE,
-        }
+        # 映射颜色字符串到内部标识
+        colormap = {'black': ReversiEnv.BLACK, 'white': ReversiEnv.WHITE}
         try:
             self.player_color = colormap[player_color]
         except KeyError:
-            raise error.Error("player_color must be 'black' or 'white', not {}".format(player_color))
-
-        self.opponent = opponent # 初始化对手对象引用
+            raise error.Error("player_color必须是'black'或'white'")
 
-        assert observation_type in ['numpy3c']
+        self.opponent = opponent  # 对手策略
+        assert observation_type in ['numpy3c'], "仅支持'numpy3c'观测类型"
         self.observation_type = observation_type
+        assert illegal_place_mode in ['lose', 'raise'], "仅支持'lose'或'raise'非法处理方式"
+        self.illegal_place_mode = illegal_place_mode
 
-        assert illegal_place_mode in ['lose', 'raise']
-        self.illegal_place_mode = illegal_place_mode # 控制如何处理非法放置操作的标志
-
-        if self.observation_type != 'numpy3c':
-            raise error.Error('Unsupported observation type: {}'.format(self.observation_type))
-
-        # One action for each board position and resign and pass
-        #这段代码主要用于 初始化强化学习环境（如游戏环境）的动作空间（action_space）和观察空间（observation_space），并完成环境的初始设置
+        # 动作空间：棋盘位置（0~size²-1）+ pass（size²）+ resign（size²+1）
         self.action_space = spaces.Discrete(self.board_size ** 2 + 2)
+        # 初始化观测空间
         observation = self.reset()
-        self.observation_space = spaces.Box(np.zeros(observation.shape), np.ones(observation.shape))
+        self.observation_space = spaces.Box(
+            low=-1.0, high=1.0, shape=observation.shape, dtype=np.float32
+        )
+
+        self._seed()  # 初始化随机种子
+
+    def reset(self):
+        """重置游戏状态，返回初始观测"""
+        # 初始化棋盘（0=空，1=黑棋，-1=白棋）
+        self.board = np.zeros((self.board_size, self.board_size), dtype=int)
+        # 中心4格初始布局
+        mid = self.board_size // 2
+        self.board[mid-1, mid-1] = 1  # 黑棋
+        self.board[mid-1, mid] = -1   # 白棋
+        self.board[mid, mid-1] = -1   # 白棋
+        self.board[mid, mid] = 1      # 黑棋
+        
+        self.current_player = 1  # 1=黑棋回合，-1=白棋回合
+        self.possible_actions = self._get_valid_moves()  # 合法动作
+        self.done = False  # 游戏结束标记
+        return self._get_observation()
+
+    def _get_valid_moves(self):
+        """获取当前玩家的合法落子位置（索引形式）"""
+        valid_moves = []
+        for i in range(self.board_size):
+            for j in range(self.board_size):
+                if self.board[i, j] == 0 and self._is_valid_move(i, j):
+                    valid_moves.append(i * self.board_size + j)
+        if not valid_moves:
+            valid_moves.append(self.board_size ** 2 + 1)  # 无合法动作时添加pass
+        return valid_moves
+
+    def _is_valid_move(self, i, j):
+        """判断(i,j)是否为当前玩家的合法落子位置"""
+        directions = [(-1, -1), (-1, 0), (-1, 1),
+                      (0, -1),          (0, 1),
+                      (1, -1),  (1, 0), (1, 1)]
+        current_color = self.current_player
+        opponent_color = -current_color
+
+        for di, dj in directions:
+            x, y = i + di, j + dj
+            flipped = []
+            while 0 <= x < self.board_size and 0 <= y < self.board_size:
+                if self.board[x, y] == opponent_color:
+                    flipped.append((x, y))
+                    x += di
+                    y += dj
+                elif self.board[x, y] == current_color:
+                    if flipped:
+                        return True
+                    break
+                else:
+                    break
+        return False
 
-        self._seed()
+    def _get_observation(self):
+        """返回3通道观测：[黑棋位置, 白棋位置, 当前玩家]"""
+        obs = np.zeros((3, self.board_size, self.board_size), dtype=np.float32)
+        obs[0] = (self.board == 1).astype(np.float32)  # 黑棋通道
+        obs[1] = (self.board == -1).astype(np.float32) # 白棋通道
+        obs[2] = np.full((self.board_size, self.board_size), self.current_player, dtype=np.float32)
+        return obs
 
-    # 设置环境的随机数种子
     def _seed(self, seed=None):
+        """设置随机种子"""
         self.np_random, seed = seeding.np_random(seed)
-
-        # Update the random policy if needed
+        # 初始化对手策略
         if isinstance(self.opponent, str):
             if self.opponent == 'random':
                 self.opponent_policy = make_random_policy(self.np_random)
-                print("################################################################")
             else:
-                raise error.Error('Unrecognized opponent policy {}'.format(self.opponent))# 如果不是可识别的对手策略，抛出错误
+                raise error.Error('不支持的对手策略: {}'.format(self.opponent))
         else:
             self.opponent_policy = self.opponent
-
         return [seed]
 
-    def _reset(self):
-        # init board setting
-        # 初始化3通道棋盘状态
-        self.state = np.zeros((3, self.board_size, self.board_size))
-        self.state[2, :, :] = 1.0
-        self.state[2, 3:5, 3:5] = 0
-        self.state[0, 4, 3] = 1
-        self.state[0, 3, 4] = 1
-        self.state[1, 3, 3] = 1
-        self.state[1, 4, 4] = 1
-        self.to_play = ReversiEnv.BLACK
-        self.possible_actions = ReversiEnv.get_possible_actions(self.state, self.to_play)
-        self.done = False
-
-        # Let the opponent play if it's not the agent's turn
-        if self.player_color != self.to_play:# 如果当前不是玩家回合(由对手回合)
-            a = self.opponent_policy(self.state)
-            ReversiEnv.make_place(self.state, a, ReversiEnv.BLACK)
-            self.to_play = ReversiEnv.WHITE
-        return self.state
-
-    def _step(self, action):
-         """
-        执行一个落子动作，并更新环境状态。
-    
-        参数:
-            action (int): 玩家选择的动作，表示棋盘上的一个位置或特殊动作（如跳过或认输）。
-                          动作空间包括:
-                            - 0 到 board_size^2 - 1: 棋盘上的具体位置 (x, y) 转换后的索引
-                            - board_size^2: 跳过（pass）
-                            - board_size^2 + 1: 认输（resign）
-    
-        返回:
-            state (np.ndarray): 更新后的棋盘状态
-            reward (float): 当前动作的即时奖励
-            done (bool): 是否游戏结束
-            info (dict): 包含额外信息的字典，如当前棋盘状态
-        """
-        color = action[1]
-        action = action[0]
-        
-        assert self.to_play == self.player_color
-        # If already terminal, then don't do anything
-        if self.done:   # 如果已经结束了
-            return self.state, 0., True, {'state': self.state}
-        if color == 0:  #  黑色棋子是 0
-            if ReversiEnv.pass_place(self.board_size, action):
-                pass
-            elif ReversiEnv.resign_place(self.board_size, action):
-                return self.state, -1, True, {'state': self.state}
-            elif not ReversiEnv.valid_place(self.state, action, self.player_color):
-                if self.illegal_place_mode == 'raise':
-                    raise
-                elif self.illegal_place_mode == 'lose':
-                    # Automatic loss on illegal place
-                    self.done = True
-                    return self.state, -1., True, {'state': self.state}
-                else:
-                    raise error.Error('Unsupported illegal place action: {}'.format(self.illegal_place_mode))
+    def step(self, action):
+        """执行动作并更新环境状态"""
+        if self.done:
+            return self._get_observation(), 0.0, True, {}
+
+        reward = 0.0
+        # 检查动作合法性
+        if not self._is_action_valid(action):
+            if self.illegal_place_mode == 'raise':
+                raise error.Error(f"非法动作: {action}（当前玩家：{self.current_player}）")
             else:
-                ReversiEnv.make_place(self.state, action, self.player_color)
-                self.possible_actions = ReversiEnv.get_possible_actions(self.state, 1)
-
-        else:       # # Opponent play  白色棋子 是 1
-            if ReversiEnv.pass_place(self.board_size, action):
-                pass
-            elif ReversiEnv.resign_place(self.board_size, action):
-                return self.state, 1, True, {'state': self.state}
-            elif not ReversiEnv.valid_place(self.state, action, 1 - self.player_color):
-                if self.illegal_place_mode == 'raise':
-                    raise
-                elif self.illegal_place_mode == 'lose':
-                    # Automatic loss on illegal place
-                    self.done = True
-                    return self.state, 1., True, {'state': self.state}
-                else:
-                    raise error.Error('Unsupported illegal place action: {}'.format(self.illegal_place_mode))
+                reward = -1.0  # 非法动作判负
+                self.done = True
+                return self._get_observation(), reward, self.done, {}
+
+        # 执行合法动作（落子/翻转）
+        if not self._is_pass_or_resign(action):
+            self._place_stone(*self._action_to_coords(action))
+
+        # 切换到对手回合
+        self.current_player *= -1
+        self.possible_actions = self._get_valid_moves()
+
+        # 对手行动
+        if not self.done:
+            opponent_action = self.opponent_policy(self._get_observation(), self.current_player)
+            if not self._is_action_valid(opponent_action):
+                reward = 1.0  # 对手非法动作，当前玩家胜
+                self.done = True
             else:
-                ReversiEnv.make_place(self.state, action , 1 - self.player_color)
-                self.possible_actions = ReversiEnv.get_possible_actions(self.state, 0 )
-
-
-        reward = ReversiEnv.game_finished(self.state)
-        if self.player_color == ReversiEnv.WHITE:
-            reward = - reward
-        self.done = reward != 0
-        return self.state, reward, self.done, {'state': self.state}
-
-    # def _reset_opponent(self):
-    #     if self.opponent == 'random':
-    #         self.opponent_policy = random_policy
-    #     else:
-    #         raise error.Error('Unrecognized opponent policy {}'.format(self.opponent))
-
-    def _render(self, mode='human', close=False):  #渲染函数，用于将当前棋盘状态可视化输出到终端或字符串中
-        if close:
-            return
-        board = self.state
+                if not self._is_pass_or_resign(opponent_action):
+                    self._place_stone(*self._action_to_coords(opponent_action))
+                # 检查游戏是否结束（双方无合法动作）
+                self.current_player *= -1
+                self.possible_actions = self._get_valid_moves()
+                if len(self.possible_actions) == 1 and self.possible_actions[0] == self.board_size**2 + 1:
+                    # 计算最终得分
+                    black_count = np.sum(self.board == 1)
+                    white_count = np.sum(self.board == -1)
+                    if black_count > white_count:
+                        reward = 1.0 if self.player_color == ReversiEnv.BLACK else -1.0
+                    elif black_count < white_count:
+                        reward = 1.0 if self.player_color == ReversiEnv.WHITE else -1.0
+                    else:
+                        reward = 0.0  # 平局
+                    self.done = True
+                self.current_player *= -1
+
+        return self._get_observation(), reward, self.done, {'board': self.board.copy()}
+
+    def render(self, mode='human'):
+        """渲染棋盘状态"""
         outfile = StringIO() if mode == 'ansi' else sys.stdout
+        d = self.board_size
 
-        outfile.write(' ' * 7)
-        for j in range(board.shape[1]):
-            outfile.write(' ' +  str(j + 1) + '  | ')
-        outfile.write('\n')
-        outfile.write(' ' * 5)
-        outfile.write('-' * (board.shape[1] * 6 - 1))# 根据列数计算分隔线长度
+        # 打印列标
+        outfile.write('   ')
+        for j in range(d):
+            outfile.write(f' {j:2} ')
         outfile.write('\n')
-        for i in range(board.shape[1]):
-            outfile.write(' ' +  str(i + 1) + '  |')
-            for j in range(board.shape[1]):
-                if board[2, i, j] == 1:
-                    outfile.write('  O  ')
-                elif board[0, i, j] == 1:
-                    outfile.write('  B  ')
+
+        # 打印棋盘
+        for i in range(d):
+            outfile.write(f'{i:2} ')  # 行标
+            for j in range(d):
+                if self.board[i, j] == 1:
+                    outfile.write(' B ')  # 黑棋
+                elif self.board[i, j] == -1:
+                    outfile.write(' W ')  # 白棋
                 else:
-                    outfile.write('  W  ')
-                outfile.write('|')
+                    outfile.write(' . ')  # 空位
             outfile.write('\n')
-            outfile.write(' ' )
-            outfile.write('-' * (board.shape[1] * 7 - 1))
-            outfile.write('\n')
-
-        if mode != 'human':
-            return outfile
 
-    # @staticmethod
-    # def pass_place(board_size, action):
-    #     return action == board_size ** 2
-
-    @staticmethod
-    def resign_place(board_size, action):
-        return action == board_size ** 2
-
-    @staticmethod
-    def pass_place(board_size, action):
-        return action == board_size ** 2 + 1
-
-    # 获取当前玩家在棋盘上所有合法的落子位置
-    @staticmethod
-    def get_possible_actions(board, player_color):
-        actions=[]
-        d = board.shape[-1]        # 棋盘维度
-        opponent_color = 1 - player_color  # 对手颜色
-
-        # 遍历棋盘上所有可落子位置
-        for pos_x in range(d):
-            for pos_y in range(d):
-                if (board[2, pos_x, pos_y]==0): # 如果该位置不是可落子点，跳过
-                    continue
-                for dx in [-1, 0, 1]:
-                    for dy in [-1, 0, 1]:
-                        if(dx == 0 and dy == 0):
-                            continue
-                        nx = pos_x + dx
-                        ny = pos_y + dy
-                        n = 0
-                        if (nx not in range(d) or ny not in range(d)): # 检查相邻位置是否在棋盘内 
-                            continue
-                        while(board[opponent_color, nx, ny] == 1):     # 沿着该方向搜索连续的对手棋子
-                            tmp_nx = nx + dx
-                            tmp_ny = ny + dy
-                            if (tmp_nx not in range(d) or tmp_ny not in range(d)):
-                                break
-                            n += 1
-                            nx += dx
-                            ny += dy
-                        if(n > 0 and board[player_color, nx, ny] == 1):
-                            actions.append(pos_x*8+pos_y)
-        if len(actions)==0:
-            actions = [d**2 + 1]
-        return actions
-
-    @staticmethod
-    def valid_reverse_opponent(board, coords, player_color):
-        '''
-        判断在指定位置落子后，是否可以翻转对手的棋子。
-    
-        参数:
-              board: 当前棋盘状态，形状为 [3, d, d]：
-            - board[0]: 黑棋位置 (Black)
-            - board[1]: 白棋位置 (White)
-            - board[2]: 可落子位置（可能未使用）
-        position: 落子的坐标 (x, y)，从 0 开始计数
-        player_color: 当前玩家颜色，0 表示黑棋，1 表示白棋
-    
-        返回:
-             bool: 是否可以翻转对手的棋子
-             list of (x, y): 所有可翻转的敌方棋子坐标列表
-        '''
-        d = board.shape[-1]
-        opponent_color = 1 - player_color
-        pos_x = coords[0]
-        pos_y = coords[1]
-        for dx in [-1, 0, 1]:
-            for dy in [-1, 0, 1]:
-                if(dx == 0 and dy == 0):
-                    continue
-                nx = pos_x + dx
-                ny = pos_y + dy
-                n = 0
-                if (nx not in range(d) or ny not in range(d)):
-                    continue
-                while(board[opponent_color, nx, ny] == 1):
-                    tmp_nx = nx + dx
-                    tmp_ny = ny + dy
-                    if (tmp_nx not in range(d) or tmp_ny not in range(d)):
-                        break
-                    n += 1
-                    nx += dx
-                    ny += dy
-                if(n > 0 and board[player_color, nx, ny] == 1):
-                    return True
-        return False
-
-    # 检查在指定位置落子是否合法
-    @staticmethod
-    def valid_place(board, action, player_color):
-        coords = ReversiEnv.action_to_coordinate(board, action)
-        # check whether there is any empty places
-        if board[2, coords[0], coords[1]] == 1:
-            # check whether there is any reversible places
-            if ReversiEnv.valid_reverse_opponent(board, coords, player_color):
-                return True
-            else:
-                return False
-        else:
-            return False
-         
-    #  在指定位置执行落子操作，并翻转被夹住的对手棋子
-    @staticmethod
-    def make_place(board, action, player_color):
-        coords = ReversiEnv.action_to_coordinate(board, action)
-
-        d = board.shape[-1]
-        opponent_color = 1 - player_color
-        pos_x = coords[0]
-        pos_y = coords[1]
-
-        for dx in [-1, 0, 1]:
-            for dy in [-1, 0, 1]:
-                if(dx == 0 and dy == 0):
-                    continue
-                nx = pos_x + dx
-                ny = pos_y + dy
-                n = 0
-                if (nx not in range(d) or ny not in range(d)):
-                    continue
-                while(board[opponent_color, nx, ny] == 1):
-                    tmp_nx = nx + dx
-                    tmp_ny = ny + dy
-                    if (tmp_nx not in range(d) or tmp_ny not in range(d)):
-                        break
-                    n += 1
-                    nx += dx
-                    ny += dy
-                if(n > 0 and board[player_color, nx, ny] == 1):
-                    nx = pos_x + dx
-                    ny = pos_y + dy
-                    while(board[opponent_color, nx, ny] == 1):
-                        board[2, nx, ny] = 0
-                        board[player_color, nx, ny] = 1
-                        board[opponent_color, nx, ny] = 0
-                        nx += dx
-                        ny += dy
-                    board[2, pos_x, pos_y] = 0
-                    board[player_color, pos_x, pos_y] = 1
-                    board[opponent_color, pos_x, pos_y] = 0
-        return board
-
-    @staticmethod
-    def coordinate_to_action(board, coords):
-        return coords[0] * board.shape[-1] + coords[1]
+        if mode == 'ansi':
+            return outfile.getvalue()
+
+    def close(self):
+        """关闭环境"""
+        pass
+
+    # 辅助方法：动作转坐标
+    def _action_to_coords(self, action):
+        if self._is_pass_or_resign(action):
+            return (-1, -1)  # 特殊动作返回无效坐标
+        return (action // self.board_size, action % self.board_size)
+
+    # 辅助方法：检查动作是否为pass或resign
+    def _is_pass_or_resign(self, action):
+        return action in [self.board_size**2, self.board_size**2 + 1]
+
+    # 辅助方法：检查动作是否合法
+    def _is_action_valid(self, action):
+        if self._is_pass_or_resign(action):
+            return True
+        i, j = self._action_to_coords(action)
+        return 0 <= i < self.board_size and 0 <= j < self.board_size and self._is_valid_move(i, j)
+
+    # 辅助方法：落子并翻转对手棋子
+    def _place_stone(self, i, j):
+        current_color = self.current_player
+        self.board[i, j] = current_color
+
+        directions = [(-1, -1), (-1, 0), (-1, 1),
+                      (0, -1),          (0, 1),
+                      (1, -1),  (1, 0), (1, 1)]
+        
+        for di, dj in directions:
+            x, y = i + di, j + dj
+            flipped = []
+            while 0 <= x < self.board_size and 0 <= y < self.board_size:
+                if self.board[x, y] == -current_color:
+                    flipped.append((x, y))
+                    x += di
+                    y += dj
+                elif self.board[x, y] == current_color:
+                    for (fx, fy) in flipped:
+                        self.board[fx, fy] = current_color
+                    break
+                else:
+                    break
 
+    # 静态方法：供外部策略获取合法动作（关键修复）
     @staticmethod
-    def action_to_coordinate(board, action):
-        return action // board.shape[-1], action % board.shape[-1]
+    def get_possible_actions(state, player_color):
+        """根据观测状态和玩家颜色返回合法动作"""
+        board_size = state.shape[-1]
+        board = np.zeros((board_size, board_size), dtype=int)
+        board[state[0] == 1.0] = 1  # 黑棋位置
+        board[state[1] == 1.0] = -1 # 白棋位置
+        
+        valid_moves = []
+        for i in range(board_size):
+            for j in range(board_size):
+                if board[i, j] == 0 and ReversiEnv._static_is_valid_move(board, i, j, player_color):
+                    valid_moves.append(i * board_size + j)
+        
+        if not valid_moves:
+            valid_moves.append(board_size ** 2 + 1)
+        return valid_moves
 
     @staticmethod
-    def game_finished(board):
-        # Returns 1 if player 1 wins, -1 if player 2 wins and 0 otherwise
-        d = board.shape[-1]
-        # 统计双方棋子数
-        player_score_x, player_score_y = np.where(board[0, :, :] == 1)
-        player_score = len(player_score_x)
-        opponent_score_x, opponent_score_y = np.where(board[1, :, :] == 1)
-        opponent_score = len(opponent_score_x)
-        # 检查是否有玩家棋子数为0
-        if player_score == 0:
-            return -1
-        elif opponent_score == 0:
-            return 1
-        else:
-            free_x, free_y = np.where(board[2, :, :] == 1)
-            if free_x.size == 0:   # 比较棋子数量决定胜负
-                if player_score > (d**2)/2:
-                    return 1
-                elif player_score == (d**2)/2:
-                    return 1
+    def _static_is_valid_move(board, i, j, player_color):
+        """静态方法：判断落子是否合法"""
+        directions = [(-1, -1), (-1, 0), (-1, 1),
+                      (0, -1),          (0, 1),
+                      (1, -1),  (1, 0), (1, 1)]
+        opponent_color = -player_color
+        board_size = board.shape[0]
+
+        for di, dj in directions:
+            x, y = i + di, j + dj
+            flipped = []
+            while 0 <= x < board_size and 0 <= y < board_size:
+                if board[x, y] == opponent_color:
+                    flipped.append((x, y))
+                    x += di
+                    y += dj
+                elif board[x, y] == player_color:
+                    if flipped:
+                        return True
+                    break
                 else:
-                    return -1
-            else:
-                return 0
-        return 0
+                    break
+        return False
\ No newline at end of file
diff --git a/src/chap14_reinforcement_learning/reversi_main.py b/src/chap14_reinforcement_learning/reversi_main.py
index a0cd4373d1..06da80e34d 100644
--- a/src/chap14_reinforcement_learning/reversi_main.py
+++ b/src/chap14_reinforcement_learning/reversi_main.py
@@ -1,122 +1,98 @@
-# 标准库
-# 导入随机数生成库，用于实现随机策略
-import random
 
-# 第三方库
-# 导入OpenAI Gym库，提供标准化的强化学习环境接口
+import random
 import gym
-# 导入NumPy库，用于高效的数值计算和数组操作
+from gym.envs.registration import register
 import numpy as np
-
-# 本地模块
-# 导入自定义的强化学习智能体类
+# 导入自定义环境和智能体
+from gym.envs.reversi.reversi import ReversiEnv
 from RL_QG_agent import RL_QG_agent
 
-# 创建黑白棋环境实例（8x8标准棋盘）
-# RL_QG_agent类实现了策略网络、价值函数和动作选择逻辑
-env = gym.make('Reversi8x8-v0')  # 使用Gym接口创建特定环境
-env.reset()  # 初始化环境状态
+# 注册黑白棋环境
+register(
+    id='Reversi8x8-v0',
+    entry_point='gym.envs.reversi.reversi:ReversiEnv',
+    kwargs={
+        'player_color': 'black',
+        'opponent': 'random',
+        'observation_type': 'numpy3c',
+        'illegal_place_mode': 'lose',
+        'board_size': 8
+    },
+    max_episode_steps=1000,  # 每局最大步数
+)
+
+# 验证环境注册
+envs = [spec.id for spec in gym.envs.registry.all()]
+print("Reversi8x8-v0 是否注册成功：", 'Reversi8x8-v0' in envs)
+
+# 创建环境
+env = gym.make(
+    'Reversi8x8-v0',
+    player_color='black',
+    opponent='random',
+    observation_type='numpy3c',
+    illegal_place_mode='lose'
+)
 
-# 初始化强化学习智能体并加载预训练模型
-agent = RL_QG_agent()  # 创建智能体实例，实现特定的学习算法
-agent.load_model()  # 加载已训练的模型参数，加速学习过程
+# 初始化智能体（白棋）
+agent = RL_QG_agent()
+agent.init_model()
+agent.load_model()
 
-# 设置训练参数
-max_epochs = 100  # 总共进行的训练局数，每局是完整的游戏
-render_interval = 10  # 每10局渲染一次，减少性能开销
+# 训练参数
+max_epochs = 10  # 训练局数（可修改）
+render_interval = 1  # 每局都渲染
 
 # 训练主循环
 for i_episode in range(max_epochs):
-    # 重置环境，开始新的一局游戏
-    # observation: 3x8x8的张量，包含游戏状态信息
-    # 3个通道分别表示: 黑棋位置、白棋位置、当前玩家
-    observation = env.reset()
-    
-    # 单局游戏循环（最多100步，防止无限循环）
-    for t in range(100):
-        # 初始化动作，稍后会被具体动作覆盖
-        action = [1, 2]  # action[0]: 落子位置(0-63)或特殊操作, action[1]: 棋子颜色(0=黑, 1=白)
+    observation = env.reset()  # 重置环境
+    for t in range(100):  # 每局最大步数
+        ################### 黑棋回合（随机策略） ###################
+        if i_episode % render_interval == 0:
+            env.render()  # 渲染棋盘
+        enables = env.possible_actions  # 合法动作
 
-        ################### 黑棋回合（使用随机策略） ###################
-        env.render()  # 可视化当前棋盘状态，便于观察训练过程
-        enables = env.possible_actions  # 获取当前合法的落子位置列表
-
-        # 处理无合法动作的情况
+        # 选择黑棋动作
         if len(enables) == 0:
-            # 无合法落子位置，执行"跳过"操作
-            action_ = env.board_size**2 + 1  # 特殊编码表示"跳过"
+            action_black = env.board_size**2 + 1  # pass
         else:
-            # 随机策略：从合法动作中随机选择一个
-            action_ = random.choice(enables)
-        
-        # 构建完整动作 [位置, 颜色]
-        action[0] = action_  # 设置落子位置
-        action[1] = 0  # 设置棋子颜色为黑色
-        
-        # 执行黑棋动作，更新环境状态
-        # observation: 更新后的环境观测
-        # reward: 执行动作后获得的即时奖励（通常为0，除非游戏结束）
-        # done: 游戏是否结束
-        # info: 包含额外信息的字典（如获胜方）
-        observation, reward, done, info = env.step(action)
+            action_black = random.choice(enables)
+
+        # 执行黑棋动作
+        observation, reward, done, info = env.step(action_black)
+        if done:
+            break
 
-        ################### 白棋回合（使用智能体策略） ###################
-        env.render()  # 再次可视化棋盘状态
-        enables = env.possible_actions  # 获取白棋合法落子位置
+        ################### 白棋回合（智能体策略） ###################
+        if i_episode % render_interval == 0:
+            env.render()
+        enables = env.possible_actions  # 白棋合法动作
 
-        # 处理无合法动作的情况
+        # 智能体选择动作
         if not enables:
-           # 当没有合法落子位置时，使用特殊值board_size**2+1表示"跳过"这一轮
-           # 这是环境API中定义的约定，用于处理一方无法行动的情况
-             action_ = env.board_size ** 2 + 1  # 执行"跳过"操作
-        else:  
-            # 使用训练好的智能体选择最佳动作
-            # observation: 当前环境观测
-            # enables: 合法动作列表
-            # 返回: 选择的动作索引
-            action_ = agent.place(observation, enables)
-        
-        # 构建完整动作
-        action[0] = action_  # 设置落子位置
-        action[1] = 1  # 设置棋子颜色为白色
-        
-        # 执行白棋动作，更新环境状态
-        # observation: 新的棋盘状态
-        # reward: 即时奖励
-        # done: 是否结束标志
-        # info: 附加信息字典
-        observation, reward, done, info = env.step(action)
+            action_white = env.board_size ** 2 + 1  # pass
+        else:
+            action_white = agent.place(observation, enables)
 
-        # 检查游戏是否结束
+        # 执行白棋动作
+        observation, reward, done, info = env.step(action_white)
         if done:
-            # 打印游戏结果摘要
-            # 使用f-string格式化输出，展示当前训练的局数和每局游戏的步数
-            # i_episode+1：将从0开始的局数索引转换为从1开始的自然计数
-           # t+1：将从0开始的步数索引转换为从1开始的自然计数，直观展示每局游戏的总步数
-            print(f"第 {i_episode+1} 局游戏在 {t+1} 步后结束")
-            
-            # 计算双方得分
-            black_score = len(np.where(env.state[0, :, :] == 1)[0])  # 统计黑棋数量
-            total_tiles = env.board_size ** 2  # 棋盘总格子数 (8x8=64)
-            
-            # 判断胜负
-            if black_score > total_tiles / 2:  # 黑棋数量超过一半
-                print("黑棋获胜！")
-            elif black_score < total_tiles / 2:  # 白棋数量超过一半
-                print("白棋获胜！")
-            else:  # 双方棋子数量相等
-                print("平局！")
-            
-            # 打印详细比分
-            white_score = total_tiles - black_score
-            print(f"比分: 黑棋 {black_score} - 白棋 {white_score}")
-            
-            break  # 结束当前游戏，开始下一局
+            break
+
+    # 游戏结束，打印结果
+    print(f"\n第 {i_episode+1} 局结束，步数：{t+1}")
+    black_score = np.sum(env.board == 1)
+    white_score = np.sum(env.board == -1)
+    print(f"黑棋：{black_score} 子，白棋：{white_score} 子")
+    if black_score > white_score:
+        print("黑棋获胜！")
+    elif black_score < white_score:
+        print("白棋获胜！")
+    else:
+        print("平局！")
 
-# 清理环境资源
-# 关闭创建的强化学习环境，释放相关的系统资源，例如关闭可能打开的文件描述符、
+# 保存最终模型并关闭环境
+agent.save_model()
 env.close()
+print(f"\n训练完成！共进行 {max_epochs} 局")
 
-print(f"训练完成！共进行了 {max_epochs} 局游戏")
-print(f"训练完成！共进行了 {max_epochs} 局游戏")
-print(f"训练完成！共进行了 {max_epochs} 局游戏")

From 6f9f826090fb21aa6b7e077bf44f6c45906c56d2 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Mon, 24 Nov 2025 09:47:50 +0800
Subject: [PATCH 06/19] =?UTF-8?q?=E4=B8=BA=E9=BB=91=E7=99=BD=E6=A3=8BRL?=
 =?UTF-8?q?=E8=AE=AD=E7=BB=83=E4=BB=A3=E7=A0=81=E6=B7=BB=E5=8A=A0=E8=AF=A6?=
 =?UTF-8?q?=E7=BB=86=E4=B8=AD=E6=96=87=E6=B3=A8=E9=87=8A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../reversi_main.py                           | 178 +++++++++++-------
 1 file changed, 110 insertions(+), 68 deletions(-)

diff --git a/src/chap14_reinforcement_learning/reversi_main.py b/src/chap14_reinforcement_learning/reversi_main.py
index 06da80e34d..d01362eec9 100644
--- a/src/chap14_reinforcement_learning/reversi_main.py
+++ b/src/chap14_reinforcement_learning/reversi_main.py
@@ -1,98 +1,140 @@
+# ==============================================
+# 黑白棋（Reversi/Othello）强化学习训练代码
+# 核心逻辑：黑棋采用随机策略，白棋采用自定义强化学习智能体（RL_QG_agent）
+# 训练流程：环境注册→环境创建→智能体初始化→多局对战训练→结果统计→模型保存
+# ==============================================
 
-import random
-import gym
-from gym.envs.registration import register
-import numpy as np
-# 导入自定义环境和智能体
-from gym.envs.reversi.reversi import ReversiEnv
-from RL_QG_agent import RL_QG_agent
+# 导入基础工具库
+import random  # 用于黑棋的随机落子策略
+import gym     # OpenAI Gym：强化学习环境标准框架，提供环境创建、交互接口
+from gym.envs.registration import register  # Gym环境注册函数，用于自定义环境注册
+import numpy as np  # 数值计算库，用于棋盘状态统计（如得分计算）
 
-# 注册黑白棋环境
+# 导入自定义模块
+from gym.envs.reversi.reversi import ReversiEnv  # 自定义黑白棋环境类（实现棋盘规则、状态管理等）
+from RL_QG_agent import RL_QG_agent  # 自定义强化学习智能体类（实现Q学习/其他RL算法）
+
+# ==============================================
+# 第一步：注册自定义黑白棋环境
+# Gym要求自定义环境必须先注册，才能通过gym.make()创建实例
+# ==============================================
 register(
-    id='Reversi8x8-v0',
-    entry_point='gym.envs.reversi.reversi:ReversiEnv',
-    kwargs={
-        'player_color': 'black',
-        'opponent': 'random',
-        'observation_type': 'numpy3c',
-        'illegal_place_mode': 'lose',
-        'board_size': 8
+    id='Reversi8x8-v0',  # 环境唯一标识符（后续创建环境时使用）
+    entry_point='gym.envs.reversi.reversi:ReversiEnv',  # 环境类的路径（包.模块:类名）
+    kwargs={  # 传递给ReversiEnv类的初始化参数
+        'player_color': 'black',  # 初始玩家颜色（黑棋先行）
+        'opponent': 'random',     # 对手类型（此处为随机策略对手，即黑棋是随机玩家）
+        'observation_type': 'numpy3c',  # 观测数据类型：3通道numpy数组（可能分别存储黑棋、白棋、空位置）
+        'illegal_place_mode': 'lose',   # 非法落子处理方式：直接判负
+        'board_size': 8  # 棋盘尺寸（8x8标准黑白棋）
     },
-    max_episode_steps=1000,  # 每局最大步数
+    max_episode_steps=1000,  # 每局最大步数限制（防止无限循环）
 )
 
-# 验证环境注册
-envs = [spec.id for spec in gym.envs.registry.all()]
-print("Reversi8x8-v0 是否注册成功：", 'Reversi8x8-v0' in envs)
+# 验证环境是否注册成功
+envs = [spec.id for spec in gym.envs.registry.all()]  # 获取所有已注册的环境ID列表
+print("Reversi8x8-v0 是否注册成功：", 'Reversi8x8-v0' in envs)  # 打印注册结果
 
-# 创建环境
+# ==============================================
+# 第二步：创建黑白棋环境实例
+# 基于已注册的环境ID，创建可交互的环境对象
+# ==============================================
 env = gym.make(
-    'Reversi8x8-v0',
-    player_color='black',
-    opponent='random',
-    observation_type='numpy3c',
-    illegal_place_mode='lose'
+    'Reversi8x8-v0',  # 目标环境ID（必须与注册时一致）
+    player_color='black',  # 覆盖注册时的参数：初始玩家为黑棋
+    opponent='random',     # 覆盖注册时的参数：对手为随机策略
+    observation_type='numpy3c',  # 观测类型：3通道numpy数组
+    illegal_place_mode='lose'    # 非法落子直接判负
 )
 
-# 初始化智能体（白棋）
-agent = RL_QG_agent()
-agent.init_model()
-agent.load_model()
+# ==============================================
+# 第三步：初始化强化学习智能体（白棋玩家）
+# ==============================================
+agent = RL_QG_agent()  # 实例化自定义RL智能体（控制白棋）
+agent.init_model()     # 初始化智能体的模型（如Q表、神经网络等）
+agent.load_model()     # 加载预训练模型（若存在，可基于历史模型继续训练）
 
-# 训练参数
-max_epochs = 10  # 训练局数（可修改）
-render_interval = 1  # 每局都渲染
+# ==============================================
+# 第四步：设置训练参数
+# ==============================================
+max_epochs = 100  # 训练总局数（可根据需求调整，如1000局、10000局）
+render_interval = 10  # 渲染间隔：每1局渲染一次棋盘（便于可视化训练过程）
 
-# 训练主循环
+# ==============================================
+# 第五步：训练主循环（核心逻辑）
+# 外层循环：每一局对战；内层循环：每一步落子（黑棋→白棋交替）
+# ==============================================
 for i_episode in range(max_epochs):
-    observation = env.reset()  # 重置环境
-    for t in range(100):  # 每局最大步数
-        ################### 黑棋回合（随机策略） ###################
+    # 重置环境：开始新一局，返回初始观测（初始棋盘状态）
+    observation = env.reset()  
+    
+    # 每局内部的步数循环（最大100步，防止超时）
+    for t in range(100):  
+        ################### 黑棋回合（随机策略玩家） ###################
+        # 按渲染间隔判断是否渲染棋盘（此处每局都渲染）
         if i_episode % render_interval == 0:
-            env.render()  # 渲染棋盘
-        enables = env.possible_actions  # 合法动作
-
-        # 选择黑棋动作
+            env.render()  # 可视化棋盘状态（显示当前落子位置、棋盘布局）
+        
+        enables = env.possible_actions  # 获取黑棋当前的所有合法落子位置（列表形式）
+        
+        # 黑棋选择动作：无合法动作则"pass"（跳过回合）
         if len(enables) == 0:
-            action_black = env.board_size**2 + 1  # pass
+            # 动作编码：棋盘大小的平方+1 表示pass（8x8棋盘→64+1=65为pass动作）
+            action_black = env.board_size**2 + 1  
         else:
-            action_black = random.choice(enables)
-
-        # 执行黑棋动作
+            action_black = random.choice(enables)  # 有合法动作时，随机选择一个落子
+        
+        # 执行黑棋动作：将选择的动作传入环境，获取反馈
+        # observation：执行动作后的新棋盘状态（观测）
+        # reward：动作带来的即时奖励（此处未显式使用，可能在智能体训练中用到）
+        # done：是否结束当前局（True=游戏结束，False=继续）
+        # info：额外信息（如落子是否合法、当前玩家等，可选）
         observation, reward, done, info = env.step(action_black)
-        if done:
+        
+        if done:  # 若黑棋动作后游戏结束（如双方都pass或棋盘满），跳出步数循环
             break
 
-        ################### 白棋回合（智能体策略） ###################
+        ################### 白棋回合（强化学习智能体） ###################
+        # 渲染棋盘（与黑棋回合一致，每局都显示）
         if i_episode % render_interval == 0:
             env.render()
-        enables = env.possible_actions  # 白棋合法动作
-
-        # 智能体选择动作
-        if not enables:
-            action_white = env.board_size ** 2 + 1  # pass
+        
+        enables = env.possible_actions  # 获取白棋当前的所有合法落子位置
+        
+        # 智能体选择动作：无合法动作则"pass"
+        if not enables:  # 等价于 len(enables) == 0
+            action_white = env.board_size ** 2 + 1  # pass动作编码
         else:
+            # 智能体根据当前观测（棋盘状态）和合法动作，选择最优落子
             action_white = agent.place(observation, enables)
-
-        # 执行白棋动作
+        
+        # 执行白棋动作：获取环境反馈
         observation, reward, done, info = env.step(action_white)
-        if done:
+        
+        if done:  # 若白棋动作后游戏结束，跳出步数循环
             break
 
-    # 游戏结束，打印结果
-    print(f"\n第 {i_episode+1} 局结束，步数：{t+1}")
-    black_score = np.sum(env.board == 1)
-    white_score = np.sum(env.board == -1)
-    print(f"黑棋：{black_score} 子，白棋：{white_score} 子")
+    # ==============================================
+    # 每局结束后：统计结果并打印
+    # ==============================================
+    print(f"\n第 {i_episode+1} 局结束，总步数：{t+1}")  # t从0开始，需+1表示实际步数
+    
+    # 计算得分：棋盘上1代表黑棋，-1代表白棋，求和得到各自棋子数量
+    black_score = np.sum(env.board == 1)    # 黑棋得分（棋子数）
+    white_score = np.sum(env.board == -1)   # 白棋得分（棋子数）
+    print(f"黑棋（随机策略）：{black_score} 子，白棋（RL智能体）：{white_score} 子")
+    
+    # 判断胜负结果
     if black_score > white_score:
-        print("黑棋获胜！")
+        print("本局结果：黑棋获胜！")
     elif black_score < white_score:
-        print("白棋获胜！")
+        print("本局结果：白棋获胜！")
     else:
-        print("平局！")
-
-# 保存最终模型并关闭环境
-agent.save_model()
-env.close()
-print(f"\n训练完成！共进行 {max_epochs} 局")
+        print("本局结果：平局！")
 
+# ==============================================
+# 第六步：训练结束后处理
+# ==============================================
+agent.save_model()  # 保存训练后的智能体模型（覆盖原有模型或保存为新文件）
+env.close()         # 关闭环境，释放资源（如渲染窗口、内存等）
+print(f"\n训练完成！共进行 {max_epochs} 局对战")
\ No newline at end of file

From 8a9642c4f5bf5f6f4bf512461a357d144c412d33 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Wed, 26 Nov 2025 00:03:20 +0800
Subject: [PATCH 07/19] =?UTF-8?q?=E5=9F=BA=E4=BA=8ECARLA=E7=9A=84=E5=A4=9A?=
 =?UTF-8?q?=E6=A8=A1=E6=80=81=E6=B3=A8=E6=84=8F=E5=8A=9BDRL=E5=AF=BC?=
 =?UTF-8?q?=E8=88=AA=E4=BB=BF=E7=9C=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 130 ++++++++++++++++++
 .../run_simulation.py                         | 103 ++++++++++++++
 2 files changed, 233 insertions(+)
 create mode 100644 src/self_driving_car_navigation/carla_environment.py
 create mode 100644 src/self_driving_car_navigation/run_simulation.py

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
new file mode 100644
index 0000000000..0f339593a2
--- /dev/null
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -0,0 +1,130 @@
+import carla
+import numpy as np
+import gym
+from queue import Queue
+import time
+import sys
+
+class CarlaEnvironment(gym.Env):
+    def __init__(self):
+        super(CarlaEnvironment, self).__init__()
+        self.client = None
+        self.world = None
+        self.blueprint_library = None
+        self._connect_carla()
+
+        self.observation_space = gym.spaces.Box(
+            low=0, high=255, shape=(128, 128, 3), dtype=np.uint8
+        )
+
+        self.vehicle = None
+        self.camera = None
+        self.image_queue = Queue(maxsize=1)
+        self.spawn_points = self.world.get_map().get_spawn_points()
+        print(f"[CARLA场景] 检测到 {len(self.spawn_points)} 个车辆生成点")
+        sys.stdout.flush()
+
+    def _connect_carla(self):
+        retry_count = 3
+        for i in range(retry_count):
+            try:
+                print(f"[CARLA连接] 尝试第{i+1}次连接（localhost:2000）...")
+                sys.stdout.flush()
+                self.client = carla.Client('localhost', 2000)
+                self.client.set_timeout(15.0)
+                self.world = self.client.get_world()
+                self.blueprint_library = self.world.get_blueprint_library()
+                print("[CARLA连接] 成功连接到模拟器")
+                sys.stdout.flush()
+                return
+            except Exception as e:
+                print(f"[CARLA连接失败] 第{i+1}次尝试：{str(e)}")
+                sys.stdout.flush()
+                if i == retry_count - 1:
+                    raise RuntimeError("超过最大重试次数，无法连接CARLA，请检查模拟器是否启动")
+                time.sleep(2)
+
+    def process_image(self, image):
+        """修复负步长和数组不可写问题"""
+        try:
+            # 1. 从原始数据解析图像（BGRA格式）
+            array = np.frombuffer(image.raw_data, dtype=np.uint8)
+            array = np.reshape(array, (image.height, image.width, 4))  # 形状：(128,128,4)
+            
+            # 2. 转换为RGB格式（关键修复：消除负步长）
+            array = array[:, :, :3]  # 移除alpha通道（保留BGR）
+            array = array[:, :, ::-1]  # BGR转RGB（这一步会产生负步长）
+            array = array.copy()  # 复制数组，消除负步长并确保可写（核心修复）
+            
+            # 3. 存入队列（避免数据堆积）
+            if self.image_queue.full():
+                self.image_queue.get()
+            self.image_queue.put(array)
+        except Exception as e:
+            print(f"[图像处理错误] {str(e)}")
+            sys.stdout.flush()
+
+    def reset(self):
+        self.close()
+        time.sleep(0.5)
+        self._spawn_vehicle()
+        if self.vehicle:
+            self._spawn_camera()
+        time.sleep(1.0)
+        return self.get_observation()
+
+    def _spawn_vehicle(self):
+        vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
+        vehicle_bp.set_attribute('color', '255,0,0')
+        vehicle_bp.set_attribute('role_name', 'drone')
+
+        for i in range(3):
+            spawn_point = self.spawn_points[i % len(self.spawn_points)] if self.spawn_points else carla.Transform()
+            print(f"[车辆生成] 尝试在生成点 {i+1} 生成车辆...")
+            sys.stdout.flush()
+            self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
+            if self.vehicle:
+                self.vehicle.set_autopilot(False)
+                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）")
+                sys.stdout.flush()
+                return
+            time.sleep(0.5)
+
+        raise RuntimeError("所有生成点尝试失败，无法生成车辆（请重启CARLA或更换场景）")
+
+    def _spawn_camera(self):
+        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.05')
+
+        camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
+        self.camera = self.world.spawn_actor(
+            camera_bp, camera_transform, attach_to=self.vehicle
+        )
+        self.camera.listen(self.process_image)
+        print("[传感器] 摄像头初始化成功")
+        sys.stdout.flush()
+
+    def get_observation(self):
+        if not self.image_queue.empty():
+            return self.image_queue.get()
+        print("[观测数据] 暂无图像，返回空帧")
+        sys.stdout.flush()
+        return np.zeros((128, 128, 3), dtype=np.uint8)
+
+    def close(self):
+        if self.camera and self.camera.is_alive:
+            self.camera.stop()
+            self.camera.destroy()
+            self.camera = None
+            print("[资源清理] 摄像头已销毁")
+            sys.stdout.flush()
+        if self.vehicle and self.vehicle.is_alive:
+            self.vehicle.destroy()
+            self.vehicle = None
+            print("[资源清理] 车辆已销毁")
+            sys.stdout.flush()
+        while not self.image_queue.empty():
+            self.image_queue.get()
\ No newline at end of file
diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py
new file mode 100644
index 0000000000..d473107f57
--- /dev/null
+++ b/src/self_driving_car_navigation/run_simulation.py
@@ -0,0 +1,103 @@
+import torch
+import time
+import sys
+from models.perception_module import PerceptionModule
+from models.attention_module import CrossDomainAttention
+from models.decision_module import DecisionModule
+from _agent.carla_environment import CarlaEnvironment
+import carla
+
+# 启动日志
+print("="*60)
+print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}")
+print(f"[Python解释器] {sys.executable}")
+print(f"[虚拟环境] {sys.prefix}")
+print("="*60)
+sys.stdout.flush()
+
+class IntegratedSystem:
+    def __init__(self, device='cpu'):
+        print("\n[模型初始化] 开始加载感知、注意力和决策模块...")
+        sys.stdout.flush()
+        self.device = device
+        try:
+            self.perception = PerceptionModule().to(self.device)
+            self.attention = CrossDomainAttention(num_blocks=6).to(self.device)
+            self.decision = DecisionModule().to(self.device)
+            print("[模型初始化] 所有模块加载完成")
+            sys.stdout.flush()
+        except Exception as e:
+            print(f"[模型初始化失败] {str(e)}")
+            sys.stdout.flush()
+            raise
+
+    def forward(self, image, lidar_data, imu_data):
+        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 torch.mean(policy, dim=1), value
+
+def run_simulation():
+    env = None
+    try:
+        print("\n[CARLA连接] 开始创建环境...")
+        sys.stdout.flush()
+        env = CarlaEnvironment()
+        
+        print("\n[环境重置] 开始生成车辆和传感器...")
+        sys.stdout.flush()
+        env.reset()
+        
+        if not env.vehicle or not env.vehicle.is_alive:
+            raise RuntimeError("车辆生成失败！请重启CARLA或更换场景（如Town03）")
+        print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}）")
+        sys.stdout.flush()
+
+        device = 'cuda' if torch.cuda.is_available() else 'cpu'
+        print(f"\n[设备信息] 模型运行在 {device} 上")
+        sys.stdout.flush()
+        system = IntegratedSystem(device=device)
+
+        print("\n[仿真开始] 共运行100步...")
+        sys.stdout.flush()
+        for step in range(100):
+            # 获取摄像头观测（已修复格式问题）
+            observation = env.get_observation()
+            if observation is None or observation.size == 0:
+                print(f"[警告] 第{step+1}步未获取到图像数据")
+                sys.stdout.flush()
+
+            # 转换为模型输入格式（确保数组可写）
+            image = torch.from_numpy(observation.copy()).permute(2, 0, 1).unsqueeze(0).float().to(device) / 255.0
+            lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(device)
+            imu_data = torch.randn(1, 6).to(device)
+
+            # 模型推理
+            policy, _ = system.forward(image, lidar_data, imu_data)
+            throttle = max(0.0, min(1.0, float(policy[0][0].clamp(-0.3, 0.8))))
+            steer = max(-1.0, min(1.0, float(policy[0][1].clamp(-0.5, 0.5))))
+
+            # 执行控制
+            control = carla.VehicleControl(throttle=throttle, steer=steer)
+            env.vehicle.apply_control(control)
+
+            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {steer:.2f}")
+            sys.stdout.flush()
+            time.sleep(0.1)
+
+        print("\n[仿真结束] 已完成100步运行")
+        sys.stdout.flush()
+
+    except Exception as e:
+        print(f"\n[仿真错误] {str(e)}")
+        sys.stdout.flush()
+    finally:
+        if env is not None:
+            print("\n[资源清理] 正在销毁车辆和传感器...")
+            sys.stdout.flush()
+            env.close()
+        print("\n[程序退出] 所有操作已完成")
+        sys.stdout.flush()
+
+if __name__ == "__main__":
+    run_simulation()
\ No newline at end of file

From 7dedf5f84cc8870dc8e3d5ad3ef55216c2f2265a Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Wed, 26 Nov 2025 23:41:41 +0800
Subject: [PATCH 08/19] =?UTF-8?q?=E4=B8=BA=E4=B8=BB=E5=87=BD=E6=95=B0?=
 =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=AF=A6=E7=BB=86=E4=B8=AD=E6=96=87=E6=B3=A8?=
 =?UTF-8?q?=E9=87=8A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/self_driving_car_navigation/main.py | 198 +++++++++++++++++++++---
 1 file changed, 175 insertions(+), 23 deletions(-)

diff --git a/src/self_driving_car_navigation/main.py b/src/self_driving_car_navigation/main.py
index 4cada34754..f7f3858fc3 100644
--- a/src/self_driving_car_navigation/main.py
+++ b/src/self_driving_car_navigation/main.py
@@ -1,82 +1,234 @@
+# 导入PyTorch核心库，用于张量操作和神经网络构建
 import torch
 import torch.nn as nn
+# 导入参数解析模块，用于命令行参数配置
 import argparse 
+# 导入Adam优化器，用于模型参数更新
 from torch.optim import Adam
+# 导入自定义数据加载器，用于加载训练/测试数据
 from utils.dataloader import get_dataloader
+# 导入自定义感知模块，负责处理多模态输入（图像、激光雷达、IMU）并提取基础特征
 from models.perception_module import PerceptionModule
+# 导入自定义跨域注意力模块，负责融合不同类型的感知特征
 from models.attention_module import CrossDomainAttention
+# 导入自定义决策模块，基于融合特征输出策略和价值估计
 from models.decision_module import DecisionModule
+# 导入自定义自评估梯度模型，用于计算动作相关的Q值估计
 from models.sagm import SelfAssessmentGradientModel  
 
 class IntegratedSystem(nn.Module):
+    """
+    多模态融合的智能决策集成系统
+    功能：整合感知、注意力融合、决策和自评估梯度模块，处理多模态输入并输出决策结果
+    输入：图像（视觉）、激光雷达（环境测距）、IMU（惯性测量）、动作（历史/目标动作）
+    输出：策略分布、价值估计、自评估Q值
+    """
     
     def __init__(self, device='cpu', state_dim=128, action_dim=2):
+        """
+        初始化集成系统的各个模块
+        Args:
+            device (str): 计算设备（'cpu'或'cuda'），默认使用CPU
+            state_dim (int): 状态特征维度，默认128（感知模块输出特征维度）
+            action_dim (int): 动作维度，默认2（例如：转向角、加速度）
+        """
         super().__init__()
+        # 记录计算设备，用于统一模块和数据的设备归属
         self.device = device
+        
+        # 初始化感知模块：处理IMU、图像、激光雷达多模态输入，提取场景关键信息
         self.perception = PerceptionModule().to(self.device)
-        self.attention = CrossDomainAttention(num_blocks=2,embed_dim=64).to(self.device)
+        
+        # 初始化跨域注意力模块：融合感知模块输出的多种特征（场景信息、分割图等）
+        # 参数：2个注意力块，嵌入维度64（特征融合后的维度）
+        self.attention = CrossDomainAttention(num_blocks=2, embed_dim=64).to(self.device)
+        
+        # 初始化决策模块：基于融合特征输出策略（动作分布）和价值（状态价值估计）
         self.decision = DecisionModule().to(self.device)
-        self.sagm = SelfAssessmentGradientModel(hidden_dim=64 ).to(self.device)  
+        
+        # 初始化自评估梯度模型：输入融合特征和动作，输出动作对应的Q值（动作价值估计）
+        # 参数：隐藏层维度64
+        self.sagm = SelfAssessmentGradientModel(hidden_dim=64).to(self.device)  
 
     def forward(self, image, lidar_data, imu_data, action):
+        """
+        前向传播：定义数据在模型中的流动和计算过程
+        Args:
+            image (torch.Tensor): 视觉图像输入，形状通常为[batch_size, channels, height, width]
+            lidar_data (torch.Tensor): 激光雷达数据输入，形状通常为[batch_size, lidar_dim, sequence_len]
+            imu_data (torch.Tensor): IMU惯性测量数据，形状通常为[batch_size, imu_dim, sequence_len]
+            action (torch.Tensor): 目标/历史动作输入，形状通常为[batch_size, action_dim]
+        Returns:
+            policy (torch.Tensor): 策略输出（动作预测），形状[batch_size, action_dim]
+            value (torch.Tensor): 状态价值估计，形状[batch_size, 1]
+            sagm_q_value (torch.Tensor): 自评估Q值（动作价值估计），形状[batch_size, 1]
+        """
+        # 1. 感知模块处理：将多模态原始输入转换为结构化特征
+        # 输出：场景信息、语义分割结果、里程计数据、障碍物检测结果、边界信息
         scene_info, segmentation, odometry, obstacles, boundary = self.perception(imu_data, image, lidar_data)
+        
+        # 2. 跨域注意力融合：融合感知模块输出的多种异质特征，得到统一的融合特征
         fused_features = self.attention(scene_info, segmentation, odometry, obstacles, boundary)
         
+        # 3. 决策模块推理：基于融合特征输出策略（动作预测）和状态价值估计
         policy, value = self.decision(fused_features)
+
+        # 4. 维度调整：对策略和价值进行序列维度平均（适应时序数据的批量处理）
+        # 假设fused_features形状为[batch_size, sequence_len, feature_dim]，需压缩sequence_len维度
+        policy = torch.mean(policy, dim=1)  # [batch_size, action_dim]
+        value = torch.mean(value, dim=1)    # [batch_size, 1]
         
-        action_3d = action.unsqueeze(1)
-        seq_len = fused_features.shape[1]
-        action_3d = action_3d.repeat(1, seq_len, 1)
+        # 5. 动作维度适配：将动作扩展为3维（匹配融合特征的时序维度），用于SAGM模块输入
+        action_3d = action.unsqueeze(1)  # [batch_size, 1, action_dim]
+        seq_len = fused_features.shape[1]  # 获取时序长度（sequence_len）
+        action_3d = action_3d.repeat(1, seq_len, 1)  # [batch_size, sequence_len, action_dim]
 
-        sagm_q_value = self.sagm(fused_features, action_3d)  
+        # 6. 自评估梯度模型推理：输入融合特征和适配后的动作，输出动作价值估计（Q值）
+        sagm_q_value = self.sagm(fused_features, action_3d) 
+        # 维度调整：对Q值进行序列维度平均
+        sagm_q_value = torch.mean(sagm_q_value, dim=1)  # [batch_size, 1]
+        
+        # 返回最终的决策输出（策略、价值、Q值）
         return policy, value, sagm_q_value
 
 def train_model(model, dataloader, optimizer, device, num_epochs=10):
+    """
+    模型训练函数：实现批量数据训练、损失计算、梯度下降更新
+    Args:
+        model (nn.Module): 待训练的集成模型（IntegratedSystem实例）
+        dataloader (DataLoader): 训练数据加载器，迭代输出批量数据
+        optimizer (torch.optim.Optimizer): 优化器（此处为Adam），用于更新模型参数
+        device (str): 计算设备（'cpu'或'cuda'）
+        num_epochs (int): 训练轮数，默认10轮
+    """
+    # 设置模型为训练模式：启用Dropout、BatchNorm更新等训练特定行为
     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)
+            # 将批量数据转移到指定计算设备（CPU/GPU），确保数据与模型设备一致
+            image = image.to(device)
+            lidar_data = lidar_data.to(device)
+            imu_data = imu_data.to(device)
+            target_action = target_action.to(device)
+            
+            # 清零优化器梯度：避免上一轮梯度累积影响当前更新
             optimizer.zero_grad()
+            
+            # 前向传播：模型输出预测结果（策略、价值、Q值）
             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()
+            # 计算损失函数：多任务损失融合（策略回归损失 + 价值估计损失 + Q值估计损失）
+            # 1. 策略损失：预测动作与目标动作的MSE损失（回归任务）
+            policy_loss = nn.MSELoss()(policy_output, target_action)
+            # 2. 价值损失：价值估计与目标动作总和的MSE损失（假设价值与动作累积效果相关）
+            value_loss = nn.MSELoss()(value_output, target_action.sum(dim=1, keepdim=True))
+            # 3. Q值损失：SAGM输出Q值与目标动作总和的MSE损失（动作价值匹配）
+            sagm_loss = nn.MSELoss()(sagm_q_value, target_action.sum(dim=1, keepdim=True))
+            # 总损失：三个子损失直接相加（可根据任务重要性调整权重）
+            total_loss = policy_loss + value_loss + sagm_loss
+            
+            # 反向传播：计算损失对模型参数的梯度
+            total_loss.backward()
+            
+            # 梯度下降：优化器更新模型参数（基于计算出的梯度）
             optimizer.step()
-            running_loss += loss.item()
+            
+            # 累积批量损失
+            running_loss += total_loss.item()
+            
+            # 每10个批量打印一次训练状态（监控训练进度）
             if i % 10 == 9:
-                print(f'Epoch [{epoch+1}/{num_epochs}], Batch [{i+1}/{len(dataloader)}], Loss: {running_loss / 10:.4f}')
+                # 计算10个批量的平均损失并打印
+                avg_batch_loss = running_loss / 10
+                print(f'Epoch [{epoch+1}/{num_epochs}], Batch [{i+1}/{len(dataloader)}], Loss: {avg_batch_loss:.4f}')
+                # 重置累计损失
                 running_loss = 0.0
+    
+    # 训练完成提示
     print('Training complete')
 
 def test_model(model, dataloader, device):
+    """
+    模型测试函数：评估模型在测试集上的性能（无梯度计算，仅前向传播）
+    Args:
+        model (nn.Module): 已训练的集成模型（IntegratedSystem实例）
+        dataloader (DataLoader): 测试数据加载器，迭代输出批量测试数据
+        device (str): 计算设备（'cpu'或'cuda'）
+    """
+    # 设置模型为评估模式：禁用Dropout、固定BatchNorm参数等测试特定行为
     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)
+            # 数据转移到指定设备
+            image = image.to(device)
+            lidar_data = lidar_data.to(device)
+            imu_data = imu_data.to(device)
+            target_action = 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}')
-import argparse
+            
+            # 计算测试损失（与训练损失计算逻辑一致，保证评估指标统一）
+            policy_loss = nn.MSELoss()(policy_output, target_action)
+            value_loss = nn.MSELoss()(value_output, target_action.sum(dim=1, keepdim=True))
+            sagm_loss = nn.MSELoss()(sagm_q_value, target_action.sum(dim=1, keepdim=True))
+            batch_loss = policy_loss + value_loss + sagm_loss
+            
+            # 累积测试损失
+            total_loss += batch_loss.item()
+    
+    # 计算测试集平均损失（总损失 / 测试批量数）
+    avg_test_loss = total_loss / len(dataloader)
+    # 打印测试结果
+    print(f'Test Average Loss: {avg_test_loss:.4f}')
 
+# 主函数入口：程序执行的起点
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--mode", type=str, default="train", choices=["train", "test"])
+    # 1. 初始化命令行参数解析器：用于接收用户输入的运行模式（训练/测试）
+    parser = argparse.ArgumentParser(description="多模态融合智能决策系统 - 训练/测试入口")
+    
+    # 添加模式参数：--mode，默认值为"train"，仅允许选择"train"或"test"
+    parser.add_argument(
+        "--mode", 
+        type=str, 
+        default="train", 
+        choices=["train", "test"],
+        help="运行模式：train（训练模型）/ test（测试模型）"
+    )
+    
+    # 解析命令行参数（获取用户输入的模式）
     args = parser.parse_args()
 
+    # 2. 自动选择计算设备：优先使用GPU（CUDA），无GPU时使用CPU
     device = "cuda" if torch.cuda.is_available() else "cpu"
+    print(f"使用计算设备: {device}")  # 打印当前使用的设备（便于调试）
+
+    # 3. 初始化模型：创建集成系统实例，指定计算设备
     model = IntegratedSystem(device=device)
+
+    # 4. 初始化优化器：使用Adam优化器，学习率0.001（可根据需求调整）
     optimizer = Adam(model.parameters(), lr=0.001)
+
+    # 5. 加载数据：通过自定义数据加载器获取训练/测试数据（数据路径、批次大小等在get_dataloader中配置）
     dataloader = get_dataloader()
+    print(f"数据加载完成，批量数: {len(dataloader)}")  # 打印批量数（便于确认数据量）
 
+    # 6. 根据运行模式执行训练或测试
     if args.mode == "train":
+        print("=== 开始模型训练 ===")
         train_model(model, dataloader, optimizer, device)
     elif args.mode == "test":
+        print("=== 开始模型测试 ===")
         test_model(model, dataloader, device)
\ No newline at end of file

From abda7b93dec66df3bbe08efa7ab399e617c9a15c Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Thu, 27 Nov 2025 18:51:19 +0800
Subject: [PATCH 09/19] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=BD=A6=E8=BE=86?=
 =?UTF-8?q?=E4=B8=8D=E5=8A=A8=E5=8F=8A=E8=BD=AE=E8=83=8E=E6=99=83=E5=8A=A8?=
 =?UTF-8?q?=E9=97=AE=E9=A2=98=EF=BC=8C=E4=BC=98=E5=8C=96=E5=9B=BE=E5=83=8F?=
 =?UTF-8?q?=E5=A4=84=E7=90=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 30 ++++++-------
 .../run_simulation.py                         | 42 ++++++++++++++-----
 2 files changed, 47 insertions(+), 25 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index 0f339593a2..902f62ae7d 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -47,16 +47,11 @@ def _connect_carla(self):
     def process_image(self, image):
         """修复负步长和数组不可写问题"""
         try:
-            # 1. 从原始数据解析图像（BGRA格式）
             array = np.frombuffer(image.raw_data, dtype=np.uint8)
-            array = np.reshape(array, (image.height, image.width, 4))  # 形状：(128,128,4)
-            
-            # 2. 转换为RGB格式（关键修复：消除负步长）
-            array = array[:, :, :3]  # 移除alpha通道（保留BGR）
-            array = array[:, :, ::-1]  # BGR转RGB（这一步会产生负步长）
-            array = array.copy()  # 复制数组，消除负步长并确保可写（核心修复）
-            
-            # 3. 存入队列（避免数据堆积）
+            array = np.reshape(array, (image.height, image.width, 4))
+            array = array[:, :, :3]  # 移除alpha通道
+            array = array[:, :, ::-1]  # BGR转RGB
+            array = array.copy()  # 消除负步长
             if self.image_queue.full():
                 self.image_queue.get()
             self.image_queue.put(array)
@@ -70,22 +65,27 @@ def reset(self):
         self._spawn_vehicle()
         if self.vehicle:
             self._spawn_camera()
-        time.sleep(1.0)
+        time.sleep(1.0)  # 等待传感器就绪
         return self.get_observation()
 
     def _spawn_vehicle(self):
+        # 选择稳定车型（特斯拉Model3）
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
-        vehicle_bp.set_attribute('color', '255,0,0')
+        vehicle_bp.set_attribute('color', '255,0,0')  # 红色，便于观察
         vehicle_bp.set_attribute('role_name', 'drone')
 
+        # 关键调整：使用第10个生成点（通常在主路中央，避免障碍物）
+        spawn_index = 10  # 可根据场景调整（0~264）
         for i in range(3):
-            spawn_point = self.spawn_points[i % len(self.spawn_points)] if self.spawn_points else carla.Transform()
-            print(f"[车辆生成] 尝试在生成点 {i+1} 生成车辆...")
+            # 优先用指定生成点，失败则重试
+            spawn_point = self.spawn_points[(spawn_index + i) % len(self.spawn_points)]
+            print(f"[车辆生成] 尝试在生成点 {spawn_index + i} 生成车辆（主路中央）...")
             sys.stdout.flush()
             self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
             if self.vehicle:
                 self.vehicle.set_autopilot(False)
-                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）")
+                self.vehicle.set_simulate_physics(True)  # 强制启用物理引擎
+                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）- 位置：主路中央")
                 sys.stdout.flush()
                 return
             time.sleep(0.5)
@@ -98,7 +98,7 @@ def _spawn_camera(self):
         camera_bp.set_attribute('image_size_y', '128')
         camera_bp.set_attribute('fov', '90')
         camera_bp.set_attribute('sensor_tick', '0.05')
-
+        # 摄像头位置：车辆前方1.5米，高度2.4米（驾驶员视角）
         camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
         self.camera = self.world.spawn_actor(
             camera_bp, camera_transform, attach_to=self.vehicle
diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py
index d473107f57..1ec17018e1 100644
--- a/src/self_driving_car_navigation/run_simulation.py
+++ b/src/self_driving_car_navigation/run_simulation.py
@@ -7,7 +7,6 @@
 from _agent.carla_environment import CarlaEnvironment
 import carla
 
-# 启动日志
 print("="*60)
 print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}")
 print(f"[Python解释器] {sys.executable}")
@@ -58,32 +57,55 @@ def run_simulation():
         sys.stdout.flush()
         system = IntegratedSystem(device=device)
 
-        print("\n[仿真开始] 共运行100步...")
+        # 新增：转向平滑参数（减少晃动）
+        steer_buffer = []  # 存储最近3步转向值
+        smooth_window = 3  # 滑动平均窗口
+        last_steer = 0.0  # 上一步转向值
+        max_steer_delta = 0.03  # 转向最大变化量（限制高频波动）
+
+        print("\n[仿真开始] 共运行100步（车辆将明显移动）...")
         sys.stdout.flush()
         for step in range(100):
-            # 获取摄像头观测（已修复格式问题）
+            # 获取摄像头观测
             observation = env.get_observation()
             if observation is None or observation.size == 0:
                 print(f"[警告] 第{step+1}步未获取到图像数据")
                 sys.stdout.flush()
 
-            # 转换为模型输入格式（确保数组可写）
+            # 转换为模型输入格式
             image = torch.from_numpy(observation.copy()).permute(2, 0, 1).unsqueeze(0).float().to(device) / 255.0
             lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(device)
             imu_data = torch.randn(1, 6).to(device)
 
             # 模型推理
             policy, _ = system.forward(image, lidar_data, imu_data)
-            throttle = max(0.0, min(1.0, float(policy[0][0].clamp(-0.3, 0.8))))
-            steer = max(-1.0, min(1.0, float(policy[0][1].clamp(-0.5, 0.5))))
 
-            # 执行控制
-            control = carla.VehicleControl(throttle=throttle, steer=steer)
+            # 关键调整1：放大油门值（确保车辆明显移动，最低0.4）
+            raw_throttle = float(policy[0][0].clamp(-0.3, 0.8))  # 原始油门
+            throttle = max(0.4, min(1.0, raw_throttle * 5))  # 放大5倍，最低0.4
+
+            # 关键调整2：平滑转向（减少轮胎晃动）
+            raw_steer = float(policy[0][1].clamp(-0.5, 0.5))  # 原始转向
+            # 滑动平均+限制变化速率
+            steer_buffer.append(raw_steer)
+            if len(steer_buffer) > smooth_window:
+                steer_buffer.pop(0)
+            smooth_steer = sum(steer_buffer) / len(steer_buffer)  # 平均
+            # 限制与上一步的差值
+            delta = smooth_steer - last_steer
+            delta = max(-max_steer_delta, min(max_steer_delta, delta))
+            final_steer = last_steer + delta
+            final_steer = max(-1.0, min(1.0, final_steer))  # 最终限制
+            last_steer = final_steer  # 更新历史值
+
+            # 执行控制指令
+            control = carla.VehicleControl(throttle=throttle, steer=final_steer)
             env.vehicle.apply_control(control)
 
-            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {steer:.2f}")
+            # 打印详细控制参数
+            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {final_steer:.2f}")
             sys.stdout.flush()
-            time.sleep(0.1)
+            time.sleep(0.1)  # 控制仿真速度
 
         print("\n[仿真结束] 已完成100步运行")
         sys.stdout.flush()

From c7f29916cde65e32ff7ecf05da47c9530fea9310 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Fri, 28 Nov 2025 10:36:25 +0800
Subject: [PATCH 10/19] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=BD=A6=E8=BE=86?=
 =?UTF-8?q?=E4=B8=8D=E5=8A=A8=E5=8F=8A=E8=BD=AE=E8=83=8E=E6=99=83=E5=8A=A8?=
 =?UTF-8?q?=E9=97=AE=E9=A2=98=EF=BC=8C=E4=BC=98=E5=8C=96=E5=9B=BE=E5=83=8F?=
 =?UTF-8?q?=E5=A4=84=E7=90=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 38 ++++++---------
 .../run_simulation.py                         | 48 ++++++++++++-------
 2 files changed, 46 insertions(+), 40 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index bd0003d3d0..902f62ae7d 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -47,18 +47,11 @@ def _connect_carla(self):
     def process_image(self, image):
         """修复负步长和数组不可写问题"""
         try:
-
-            # 1. 从原始数据解析图像（BGRA格式）
             array = np.frombuffer(image.raw_data, dtype=np.uint8)
-            array = np.reshape(array, (image.height, image.width, 4))  # 形状：(128,128,4)
-            
-            # 2. 转换为RGB格式（关键修复：消除负步长）
-            array = array[:, :, :3]  # 移除alpha通道（保留BGR）
-            array = array[:, :, ::-1]  # BGR转RGB（这一步会产生负步长）
-            array = array.copy()  # 复制数组，消除负步长并确保可写（核心修复）
-            
-            # 3. 存入队列（避免数据堆积）
-
+            array = np.reshape(array, (image.height, image.width, 4))
+            array = array[:, :, :3]  # 移除alpha通道
+            array = array[:, :, ::-1]  # BGR转RGB
+            array = array.copy()  # 消除负步长
             if self.image_queue.full():
                 self.image_queue.get()
             self.image_queue.put(array)
@@ -72,26 +65,27 @@ def reset(self):
         self._spawn_vehicle()
         if self.vehicle:
             self._spawn_camera()
-                                                                 
-        time.sleep(1.0)
+        time.sleep(1.0)  # 等待传感器就绪
         return self.get_observation()
 
     def _spawn_vehicle(self):
+        # 选择稳定车型（特斯拉Model3）
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
-        vehicle_bp.set_attribute('color', '255,0,0')
+        vehicle_bp.set_attribute('color', '255,0,0')  # 红色，便于观察
         vehicle_bp.set_attribute('role_name', 'drone')
 
+        # 关键调整：使用第10个生成点（通常在主路中央，避免障碍物）
+        spawn_index = 10  # 可根据场景调整（0~264）
         for i in range(3):
-            spawn_point = self.spawn_points[i % len(self.spawn_points)] if self.spawn_points else carla.Transform()
-            print(f"[车辆生成] 尝试在生成点 {i+1} 生成车辆...")
-
+            # 优先用指定生成点，失败则重试
+            spawn_point = self.spawn_points[(spawn_index + i) % len(self.spawn_points)]
+            print(f"[车辆生成] 尝试在生成点 {spawn_index + i} 生成车辆（主路中央）...")
             sys.stdout.flush()
             self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
             if self.vehicle:
                 self.vehicle.set_autopilot(False)
-
-                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）")
-
+                self.vehicle.set_simulate_physics(True)  # 强制启用物理引擎
+                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）- 位置：主路中央")
                 sys.stdout.flush()
                 return
             time.sleep(0.5)
@@ -104,9 +98,7 @@ def _spawn_camera(self):
         camera_bp.set_attribute('image_size_y', '128')
         camera_bp.set_attribute('fov', '90')
         camera_bp.set_attribute('sensor_tick', '0.05')
-
-
-
+        # 摄像头位置：车辆前方1.5米，高度2.4米（驾驶员视角）
         camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
         self.camera = self.world.spawn_actor(
             camera_bp, camera_transform, attach_to=self.vehicle
diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py
index da602758e5..1ec17018e1 100644
--- a/src/self_driving_car_navigation/run_simulation.py
+++ b/src/self_driving_car_navigation/run_simulation.py
@@ -7,9 +7,6 @@
 from _agent.carla_environment import CarlaEnvironment
 import carla
 
-
-# 启动日志
-
 print("="*60)
 print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}")
 print(f"[Python解释器] {sys.executable}")
@@ -60,20 +57,22 @@ def run_simulation():
         sys.stdout.flush()
         system = IntegratedSystem(device=device)
 
+        # 新增：转向平滑参数（减少晃动）
+        steer_buffer = []  # 存储最近3步转向值
+        smooth_window = 3  # 滑动平均窗口
+        last_steer = 0.0  # 上一步转向值
+        max_steer_delta = 0.03  # 转向最大变化量（限制高频波动）
 
-        print("\n[仿真开始] 共运行100步...")
+        print("\n[仿真开始] 共运行100步（车辆将明显移动）...")
         sys.stdout.flush()
         for step in range(100):
-            # 获取摄像头观测（已修复格式问题）
-
+            # 获取摄像头观测
             observation = env.get_observation()
             if observation is None or observation.size == 0:
                 print(f"[警告] 第{step+1}步未获取到图像数据")
                 sys.stdout.flush()
 
-
-            # 转换为模型输入格式（确保数组可写）
-
+            # 转换为模型输入格式
             image = torch.from_numpy(observation.copy()).permute(2, 0, 1).unsqueeze(0).float().to(device) / 255.0
             lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(device)
             imu_data = torch.randn(1, 6).to(device)
@@ -81,17 +80,32 @@ def run_simulation():
             # 模型推理
             policy, _ = system.forward(image, lidar_data, imu_data)
 
-            throttle = max(0.0, min(1.0, float(policy[0][0].clamp(-0.3, 0.8))))
-            steer = max(-1.0, min(1.0, float(policy[0][1].clamp(-0.5, 0.5))))
-
-            # 执行控制
-            control = carla.VehicleControl(throttle=throttle, steer=steer)
+            # 关键调整1：放大油门值（确保车辆明显移动，最低0.4）
+            raw_throttle = float(policy[0][0].clamp(-0.3, 0.8))  # 原始油门
+            throttle = max(0.4, min(1.0, raw_throttle * 5))  # 放大5倍，最低0.4
+
+            # 关键调整2：平滑转向（减少轮胎晃动）
+            raw_steer = float(policy[0][1].clamp(-0.5, 0.5))  # 原始转向
+            # 滑动平均+限制变化速率
+            steer_buffer.append(raw_steer)
+            if len(steer_buffer) > smooth_window:
+                steer_buffer.pop(0)
+            smooth_steer = sum(steer_buffer) / len(steer_buffer)  # 平均
+            # 限制与上一步的差值
+            delta = smooth_steer - last_steer
+            delta = max(-max_steer_delta, min(max_steer_delta, delta))
+            final_steer = last_steer + delta
+            final_steer = max(-1.0, min(1.0, final_steer))  # 最终限制
+            last_steer = final_steer  # 更新历史值
+
+            # 执行控制指令
+            control = carla.VehicleControl(throttle=throttle, steer=final_steer)
             env.vehicle.apply_control(control)
 
-            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {steer:.2f}")
+            # 打印详细控制参数
+            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {final_steer:.2f}")
             sys.stdout.flush()
-            time.sleep(0.1)
-
+            time.sleep(0.1)  # 控制仿真速度
 
         print("\n[仿真结束] 已完成100步运行")
         sys.stdout.flush()

From ef62ae6854c1c62d0cb1c469c79e50fd5113a54c Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Sat, 29 Nov 2025 22:57:34 +0800
Subject: [PATCH 11/19] =?UTF-8?q?=E4=BC=98=E5=8C=96=E9=9A=9C=E7=A2=8D?=
 =?UTF-8?q?=E7=89=A9=E6=A3=80=E6=B5=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../attention_module.py                       | 18 +++++------
 .../perception_module.py                      | 31 +++++++++++++++----
 2 files changed, 34 insertions(+), 15 deletions(-)

diff --git a/src/self_driving_car_navigation/attention_module.py b/src/self_driving_car_navigation/attention_module.py
index 8faa2bf93e..6b0c790b0b 100644
--- a/src/self_driving_car_navigation/attention_module.py
+++ b/src/self_driving_car_navigation/attention_module.py
@@ -3,15 +3,15 @@
 from torch.nn import MultiheadAttention
 
 class CrossDomainAttention(nn.Module):
-    def __init__(self, num_blocks=2, embed_dim=64):  # 关键修改：将embed_dim改为256（与输入特征维度匹配）
+    def __init__(self, num_blocks=2, embed_dim=256):  # 确保embed_dim与感知模块输出匹配
         super().__init__()
         self.num_blocks = num_blocks
         # 多头注意力层的embed_dim必须与输入特征的最后一维一致
         self.attention_blocks = nn.ModuleList([
-            MultiheadAttention(embed_dim=embed_dim, num_heads=2, batch_first=True)  # 使用传入的embed_dim
+            MultiheadAttention(embed_dim=embed_dim, num_heads=2, batch_first=True)
             for _ in range(num_blocks)
         ])
-        self.norm = nn.LayerNorm(embed_dim)  # 层归一化的维度也需匹配
+        self.norm = nn.LayerNorm(embed_dim)  # 层归一化维度与embed_dim一致
 
     def forward(self, scene_info, segmentation, odometry, obstacles, boundary):
         # 统一输入张量维度为3D [batch, seq_len, features]
@@ -35,22 +35,22 @@ def adjust_dim(tensor):
             adjust_dim(boundary)
         ]
 
-        # 统一所有特征的最后一维（特征维度）为 embed_dim（256）
-        target_feat_dim = self.attention_blocks[0].embed_dim  # 从注意力模块获取目标维度（256）
+        # 统一所有特征的最后一维（特征维度）为 embed_dim
+        target_feat_dim = self.attention_blocks[0].embed_dim
         adjusted_inputs = []
         for x in inputs:
             if x.shape[-1] != target_feat_dim:
-                # 用线性层将特征维度转换为目标维度（256）
+                # 动态创建线性层并转移到相同设备
                 linear = nn.Linear(x.shape[-1], target_feat_dim, device=x.device)
                 x = linear(x)
             adjusted_inputs.append(x)
 
         # 拼接所有特征（在seq_len维度拼接）
-        x = torch.cat(adjusted_inputs, dim=1)  # 此时x的形状为 [batch, total_seq_len, 256]
+        x = torch.cat(adjusted_inputs, dim=1)
 
-        # 注意力计算（此时输入维度与embed_dim匹配）
+        # 注意力计算
         for attn_block in self.attention_blocks:
-            attn_output, _ = attn_block(x, x, x)  # 自注意力计算
+            attn_output, _ = attn_block(x, x, x)  # 自注意力
             x = self.norm(x + attn_output)  # 残差连接 + 层归一化
 
         return x
\ No newline at end of file
diff --git a/src/self_driving_car_navigation/perception_module.py b/src/self_driving_car_navigation/perception_module.py
index 7f06d236d1..27bc8c27bf 100644
--- a/src/self_driving_car_navigation/perception_module.py
+++ b/src/self_driving_car_navigation/perception_module.py
@@ -4,6 +4,7 @@
 class PerceptionModule(nn.Module):
     def __init__(self):
         super(PerceptionModule, self).__init__()
+        # 原有语义分割网络保持不变
         self.segmentation_net = nn.Sequential(
             nn.Conv2d(3, 16, kernel_size=3, stride=1, padding=1),
             nn.ReLU(),
@@ -15,12 +16,30 @@ def __init__(self):
             nn.ReLU(),
             nn.MaxPool2d(kernel_size=2, stride=2)
         )
+        
+        # 修正：激光雷达障碍物检测子网络（输入通道从256改为1，适配实际激光雷达通道数）
+        self.obstacle_net = nn.Sequential(
+            nn.Conv1d(in_channels=1, out_channels=128, kernel_size=3, padding=1),  # 输入通道=1（激光雷达数据通道数）
+            nn.ReLU(),
+            nn.MaxPool1d(kernel_size=2),
+            nn.Conv1d(in_channels=128, out_channels=64, kernel_size=3, padding=1),
+            nn.ReLU(),
+            nn.AdaptiveMaxPool1d(output_size=16),  # 固定输出长度
+            nn.Flatten()  # 展平为特征向量
+        )
 
     def forward(self, imu_data, image, lidar_data):
+        # 原有视觉特征提取保持不变
         segmentation = self.segmentation_net(image)
-        scene_info = segmentation.mean(dim=(2, 3))  # 视觉信息提取
-        odometry = imu_data  # IMU用于里程计
-        obstacles = lidar_data.mean(dim=1)  # 障碍物检测
-        boundary = lidar_data.max(dim=1)[0]  # 边界检测
-
-        return scene_info, segmentation, odometry, obstacles, boundary
+        scene_info = segmentation.mean(dim=(2, 3))  # 视觉场景信息
+        odometry = imu_data  # IMU里程计数据
+        boundary = lidar_data.max(dim=1)[0]  # 边界检测保持不变
+        
+        # 关键修改：将激光雷达4D数据[batch, 1, 64, 64]转为3D[batch, 1, 64*64]，适配1D卷积
+        # 展平后两个空间维度（64x64 → 4096）
+        lidar_reshaped = lidar_data.flatten(start_dim=2)  # 形状变为[batch, 1, 64*64=4096]
+        
+        # 用专用子网络处理激光雷达数据，提取障碍物特征
+        obstacles = self.obstacle_net(lidar_reshaped)  # 输出[batch, 64*16=1024]
+        
+        return scene_info, segmentation, odometry, obstacles, boundary
\ No newline at end of file

From 455c84d6382216b391220a68418f69f2048c59ce Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Sun, 30 Nov 2025 13:05:37 +0800
Subject: [PATCH 12/19] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=BB=B4=E5=BA=A6?=
 =?UTF-8?q?=E4=B8=8D=E5=8C=B9=E9=85=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/self_driving_car_navigation/decision_module.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/self_driving_car_navigation/decision_module.py b/src/self_driving_car_navigation/decision_module.py
index 147d9664e0..bd2ee88d3e 100644
--- a/src/self_driving_car_navigation/decision_module.py
+++ b/src/self_driving_car_navigation/decision_module.py
@@ -5,14 +5,14 @@ class DecisionModule(nn.Module):
     def __init__(self):
         super(DecisionModule, self).__init__()
         self.policy_net = nn.Sequential(
-            nn.Linear(64, 128),
+            nn.Linear(256, 128),  # 64改为256，与注意力输出匹配
             nn.ReLU(),
             nn.Linear(128, 64),
             nn.ReLU(),
             nn.Linear(64, 2)  # 输出：转向角和油门
         )
         self.value_net = nn.Sequential(
-            nn.Linear(64, 128),
+            nn.Linear(256, 128),  # 64改为256，与注意力输出匹配
             nn.ReLU(),
             nn.Linear(128, 64),
             nn.ReLU(),

From 76be5d12d05a6a663b9dc93b65bc7af1aa864502 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Mon, 1 Dec 2025 11:33:07 +0800
Subject: [PATCH 13/19] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E9=95=9C=E5=A4=B4?=
 =?UTF-8?q?=E5=9B=BA=E5=AE=9A=E8=BD=A6=E8=BE=86=E6=AD=A3=E5=90=8E=E6=96=B9?=
 =?UTF-8?q?5m+=E4=B8=8A=E6=96=B92m=E5=AE=9E=E6=97=B6=E8=B7=9F=E9=9A=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../run_simulation.py                         | 60 +++++++++++++------
 1 file changed, 42 insertions(+), 18 deletions(-)

diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py
index 1ec17018e1..500e117e14 100644
--- a/src/self_driving_car_navigation/run_simulation.py
+++ b/src/self_driving_car_navigation/run_simulation.py
@@ -1,6 +1,7 @@
 import torch
 import time
 import sys
+import math
 from models.perception_module import PerceptionModule
 from models.attention_module import CrossDomainAttention
 from models.decision_module import DecisionModule
@@ -52,18 +53,21 @@ def run_simulation():
         print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}）")
         sys.stdout.flush()
 
+        # 获取CARLA可视化镜头控制器
+        spectator = env.world.get_spectator()
+
         device = 'cuda' if torch.cuda.is_available() else 'cpu'
         print(f"\n[设备信息] 模型运行在 {device} 上")
         sys.stdout.flush()
         system = IntegratedSystem(device=device)
 
-        # 新增：转向平滑参数（减少晃动）
-        steer_buffer = []  # 存储最近3步转向值
-        smooth_window = 3  # 滑动平均窗口
-        last_steer = 0.0  # 上一步转向值
-        max_steer_delta = 0.03  # 转向最大变化量（限制高频波动）
+        # 转向平滑参数
+        steer_buffer = []
+        smooth_window = 3
+        last_steer = 0.0
+        max_steer_delta = 0.03
 
-        print("\n[仿真开始] 共运行100步（车辆将明显移动）...")
+        print("\n[仿真开始] 共运行100步（镜头固定正后方5m、上方2m，实时跟随）...")
         sys.stdout.flush()
         for step in range(100):
             # 获取摄像头观测
@@ -80,32 +84,52 @@ def run_simulation():
             # 模型推理
             policy, _ = system.forward(image, lidar_data, imu_data)
 
-            # 关键调整1：放大油门值（确保车辆明显移动，最低0.4）
-            raw_throttle = float(policy[0][0].clamp(-0.3, 0.8))  # 原始油门
-            throttle = max(0.4, min(1.0, raw_throttle * 5))  # 放大5倍，最低0.4
+            # 放大油门
+            raw_throttle = float(policy[0][0].clamp(-0.3, 0.8))
+            throttle = max(0.4, min(1.0, raw_throttle * 5))
 
-            # 关键调整2：平滑转向（减少轮胎晃动）
-            raw_steer = float(policy[0][1].clamp(-0.5, 0.5))  # 原始转向
-            # 滑动平均+限制变化速率
+            # 平滑转向
+            raw_steer = float(policy[0][1].clamp(-0.5, 0.5))
             steer_buffer.append(raw_steer)
             if len(steer_buffer) > smooth_window:
                 steer_buffer.pop(0)
-            smooth_steer = sum(steer_buffer) / len(steer_buffer)  # 平均
-            # 限制与上一步的差值
+            smooth_steer = sum(steer_buffer) / len(steer_buffer)
             delta = smooth_steer - last_steer
             delta = max(-max_steer_delta, min(max_steer_delta, delta))
             final_steer = last_steer + delta
-            final_steer = max(-1.0, min(1.0, final_steer))  # 最终限制
-            last_steer = final_steer  # 更新历史值
+            final_steer = max(-1.0, min(1.0, final_steer))
+            last_steer = final_steer
 
             # 执行控制指令
             control = carla.VehicleControl(throttle=throttle, steer=final_steer)
             env.vehicle.apply_control(control)
 
-            # 打印详细控制参数
+            # ========== 修复API错误：手动计算镜头朝向（严格正后方+看向车辆） ==========
+            vehicle_transform = env.vehicle.get_transform()
+            # 1. 车辆局部坐标系转世界坐标系（严格正后方5m、上方2m）
+            local_cam_loc = carla.Location(x=-5.0, y=0.0, z=2.0)  # 车辆自身正后方
+            camera_location = vehicle_transform.transform(local_cam_loc)
+            
+            # 2. 手动计算镜头朝向（看向车辆）
+            # 计算镜头到车辆的方向向量
+            dir_x = vehicle_transform.location.x - camera_location.x
+            dir_y = vehicle_transform.location.y - camera_location.y
+            dir_z = vehicle_transform.location.z - camera_location.z
+            # 计算水平旋转角yaw（绕z轴）
+            yaw = math.atan2(dir_y, dir_x) * 180 / math.pi
+            # 计算俯仰角pitch（绕y轴，轻微俯视）
+            pitch = -10.0  # 固定俯视10度，确保看到车辆
+            # 构建旋转对象
+            camera_rotation = carla.Rotation(pitch=pitch, yaw=yaw, roll=0.0)
+            
+            # 3. 应用镜头设置
+            spectator.set_transform(carla.Transform(camera_location, camera_rotation))
+            # ======================================================================
+
+            # 打印控制参数
             print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {final_steer:.2f}")
             sys.stdout.flush()
-            time.sleep(0.1)  # 控制仿真速度
+            time.sleep(0.1)
 
         print("\n[仿真结束] 已完成100步运行")
         sys.stdout.flush()

From e9a381d13d2ee443abf8faab83a673c4dbe47099 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Thu, 4 Dec 2025 23:32:11 +0800
Subject: [PATCH 14/19] =?UTF-8?q?=E4=BC=98=E5=8C=96=E8=87=AA=E5=8A=A8?=
 =?UTF-8?q?=E9=A9=BE=E9=A9=B6=E9=81=BF=E9=9A=9C=E4=B8=8E=E6=8E=A7=E5=88=B6?=
 =?UTF-8?q?=E7=AD=96=E7=95=A5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 201 +++++++++++++-----
 .../perception_module.py                      |  62 +++---
 .../run_simulation.py                         | 186 ++++++++--------
 3 files changed, 281 insertions(+), 168 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index 902f62ae7d..d3305f14b6 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -1,9 +1,9 @@
+import gym
 import carla
 import numpy as np
-import gym
-from queue import Queue
-import time
 import sys
+import time
+from queue import Queue
 
 class CarlaEnvironment(gym.Env):
     def __init__(self):
@@ -13,118 +13,209 @@ def __init__(self):
         self.blueprint_library = None
         self._connect_carla()
 
-        self.observation_space = gym.spaces.Box(
-            low=0, high=255, shape=(128, 128, 3), dtype=np.uint8
-        )
+        # 观测空间定义
+        self.observation_space = gym.spaces.Dict({
+            'image': gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8),
+            'lidar_distances': gym.spaces.Box(low=0, high=50, shape=(360,), dtype=np.float32),
+            'imu': gym.spaces.Box(low=-10, high=10, shape=(6,), dtype=np.float32)
+        })
 
+        # 传感器和车辆实例
         self.vehicle = None
         self.camera = None
+        self.lidar = None
+        self.imu = None
+        # 数据队列
         self.image_queue = Queue(maxsize=1)
+        self.lidar_queue = Queue(maxsize=1)
+        self.imu_queue = Queue(maxsize=1)
+        # 生成点
         self.spawn_points = self.world.get_map().get_spawn_points()
         print(f"[CARLA场景] 检测到 {len(self.spawn_points)} 个车辆生成点")
         sys.stdout.flush()
 
     def _connect_carla(self):
+        """连接CARLA服务器，支持重试"""
         retry_count = 3
         for i in range(retry_count):
             try:
                 print(f"[CARLA连接] 尝试第{i+1}次连接（localhost:2000）...")
-                sys.stdout.flush()
                 self.client = carla.Client('localhost', 2000)
-                self.client.set_timeout(15.0)
+                self.client.set_timeout(15.0)  # 超时时间15秒
                 self.world = self.client.get_world()
                 self.blueprint_library = self.world.get_blueprint_library()
                 print("[CARLA连接] 成功连接到模拟器")
-                sys.stdout.flush()
                 return
             except Exception as e:
-                print(f"[CARLA连接失败] 第{i+1}次尝试：{str(e)}")
-                sys.stdout.flush()
+                print(f"[CARLA连接失败] {str(e)}")
                 if i == retry_count - 1:
-                    raise RuntimeError("超过最大重试次数，无法连接CARLA，请检查模拟器是否启动")
+                    raise RuntimeError("无法连接CARLA，请检查模拟器是否启动")
                 time.sleep(2)
 
     def process_image(self, image):
-        """修复负步长和数组不可写问题"""
+        """处理摄像头数据，转换为RGB格式"""
         try:
             array = np.frombuffer(image.raw_data, dtype=np.uint8)
-            array = np.reshape(array, (image.height, image.width, 4))
+            array = np.reshape(array, (image.height, image.width, 4))  # (H,W,4)
             array = array[:, :, :3]  # 移除alpha通道
-            array = array[:, :, ::-1]  # BGR转RGB
-            array = array.copy()  # 消除负步长
+            array = array[:, :, ::-1].copy()  # BGR转RGB
             if self.image_queue.full():
                 self.image_queue.get()
             self.image_queue.put(array)
         except Exception as e:
             print(f"[图像处理错误] {str(e)}")
-            sys.stdout.flush()
+
+    def process_lidar(self, data):
+        """处理激光雷达数据，生成360度距离数组"""
+        try:
+            # 解析点云数据 (x,y,z,intensity)
+            points = np.frombuffer(data.raw_data, dtype=np.dtype('f4')).reshape(-1, 4)[:, :3]
+            distances = np.linalg.norm(points, axis=1)  # 计算每个点到车辆的距离
+            angles = np.arctan2(points[:, 1], points[:, 0]) * 180 / np.pi  # 计算角度（度）
+            angles = (angles + 360) % 360  # 归一化到0-360度
+
+            # 初始化360度距离数组（默认50米）
+            lidar_distances = np.full(360, 50.0, dtype=np.float32)
+            # 填充每个角度的最近距离
+            for angle, dist in zip(angles, distances):
+                angle_idx = int(round(angle)) % 360
+                if dist < lidar_distances[angle_idx]:
+                    lidar_distances[angle_idx] = dist
+
+            if self.lidar_queue.full():
+                self.lidar_queue.get()
+            self.lidar_queue.put(lidar_distances)
+        except Exception as e:
+            print(f"[激光雷达处理错误] {str(e)}")
+
+    def process_imu(self, data):
+        """处理IMU数据，提取加速度和角速度"""
+        try:
+            imu_data = np.array([
+                data.accelerometer.x, data.accelerometer.y, data.accelerometer.z,
+                data.gyroscope.x, data.gyroscope.y, data.gyroscope.z
+            ], dtype=np.float32)
+            if self.imu_queue.full():
+                self.imu_queue.get()
+            self.imu_queue.put(imu_data)
+        except Exception as e:
+            print(f"[IMU处理错误] {str(e)}")
 
     def reset(self):
+        """重置环境，生成车辆和传感器"""
         self.close()
         time.sleep(0.5)
         self._spawn_vehicle()
         if self.vehicle:
-            self._spawn_camera()
+            self._spawn_sensors()
         time.sleep(1.0)  # 等待传感器就绪
         return self.get_observation()
 
     def _spawn_vehicle(self):
-        # 选择稳定车型（特斯拉Model3）
+        """生成车辆（特斯拉Model3）"""
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
-        vehicle_bp.set_attribute('color', '255,0,0')  # 红色，便于观察
-        vehicle_bp.set_attribute('role_name', 'drone')
+        vehicle_bp.set_attribute('color', '255,0,0')  # 红色
+        vehicle_bp.set_attribute('role_name', 'ego_vehicle')
 
-        # 关键调整：使用第10个生成点（通常在主路中央，避免障碍物）
-        spawn_index = 10  # 可根据场景调整（0~264）
+        # 选择主路生成点（降低初始碰撞概率）
+        spawn_index = 10
         for i in range(3):
-            # 优先用指定生成点，失败则重试
             spawn_point = self.spawn_points[(spawn_index + i) % len(self.spawn_points)]
-            print(f"[车辆生成] 尝试在生成点 {spawn_index + i} 生成车辆（主路中央）...")
-            sys.stdout.flush()
             self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
             if self.vehicle:
                 self.vehicle.set_autopilot(False)
-                self.vehicle.set_simulate_physics(True)  # 强制启用物理引擎
-                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）- 位置：主路中央")
-                sys.stdout.flush()
+                self.vehicle.set_simulate_physics(True)
+                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）")
                 return
-            time.sleep(0.5)
+        raise RuntimeError("车辆生成失败，请重启CARLA或更换场景（如Town03）")
 
-        raise RuntimeError("所有生成点尝试失败，无法生成车辆（请重启CARLA或更换场景）")
-
-    def _spawn_camera(self):
+    def _spawn_sensors(self):
+        """生成摄像头、激光雷达、IMU传感器（核心修正：兼容激光雷达参数）"""
+        # 1. 前视摄像头
         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.05')
-        # 摄像头位置：车辆前方1.5米，高度2.4米（驾驶员视角）
-        camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
         self.camera = self.world.spawn_actor(
-            camera_bp, camera_transform, attach_to=self.vehicle
+            camera_bp, carla.Transform(carla.Location(x=1.5, z=2.4)), attach_to=self.vehicle
         )
         self.camera.listen(self.process_image)
-        print("[传感器] 摄像头初始化成功")
-        sys.stdout.flush()
+
+        # 2. 激光雷达（关键修正：用upper_fov和lower_fov替代vertical_fov）
+        lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast')
+        lidar_bp.set_attribute('channels', '64')  # 64线
+        lidar_bp.set_attribute('range', '50')  # 最大50米
+        lidar_bp.set_attribute('points_per_second', '200000')
+        lidar_bp.set_attribute('rotation_frequency', '20')  # 20Hz
+        lidar_bp.set_attribute('horizontal_fov', '360')  # 全向扫描
+        # 垂直角度范围：-30°到30°（兼容所有版本）
+        lidar_bp.set_attribute('upper_fov', '30.0')    # 上角度
+        lidar_bp.set_attribute('lower_fov', '-30.0')   # 下角度
+        self.lidar = self.world.spawn_actor(
+            lidar_bp, carla.Transform(carla.Location(x=0.0, z=2.0)), attach_to=self.vehicle
+        )
+        self.lidar.listen(self.process_lidar)
+
+        # 3. IMU传感器
+        imu_bp = self.blueprint_library.find('sensor.other.imu')
+        imu_bp.set_attribute('sensor_tick', '0.05')
+        self.imu = self.world.spawn_actor(
+            imu_bp, carla.Transform(), attach_to=self.vehicle
+        )
+        self.imu.listen(self.process_imu)
+        print("[传感器] 全向激光雷达+摄像头+IMU初始化成功")
 
     def get_observation(self):
-        if not self.image_queue.empty():
-            return self.image_queue.get()
-        print("[观测数据] 暂无图像，返回空帧")
-        sys.stdout.flush()
-        return np.zeros((128, 128, 3), dtype=np.uint8)
+        """获取传感器数据（确保数据就绪）"""
+        while self.image_queue.empty() or self.lidar_queue.empty() or self.imu_queue.empty():
+            time.sleep(0.01)
+        return {
+            'image': self.image_queue.get(),
+            'lidar_distances': self.lidar_queue.get(),
+            'imu': self.imu_queue.get()
+        }
+
+    def get_obstacle_directions(self, lidar_distances):
+        """计算前/后/左/右四个方向的最近障碍物距离"""
+        # 角度范围定义（度）
+        front_angles = np.concatenate([np.arange(345, 360), np.arange(0, 16)])  # 前方：-15~15°
+        rear_angles = np.arange(165, 196)  # 后方：165~195°
+        left_angles = np.arange(75, 106)  # 左方：75~105°
+        right_angles = np.arange(255, 286)  # 右方：255~285°（-105~-75°）
+
+        return {
+            'front': np.min(lidar_distances[front_angles]),
+            'rear': np.min(lidar_distances[rear_angles]),
+            'left': np.min(lidar_distances[left_angles]),
+            'right': np.min(lidar_distances[right_angles])
+        }
+
+    def step(self, action):
+        """执行动作并返回环境反馈"""
+        control = carla.VehicleControl(
+            throttle=float(action[0]),
+            steer=float(action[1]),
+            brake=float(action[2])
+        )
+        self.vehicle.apply_control(control)
+        observation = self.get_observation()
+        reward = 1.0  # 基础存活奖励
+        done = False
+        return observation, reward, done, {}
 
     def close(self):
-        if self.camera and self.camera.is_alive:
-            self.camera.stop()
-            self.camera.destroy()
-            self.camera = None
-            print("[资源清理] 摄像头已销毁")
-            sys.stdout.flush()
-        if self.vehicle and self.vehicle.is_alive:
+        """清理资源（传感器和车辆）"""
+        # 销毁传感器
+        for sensor in [self.camera, self.lidar, self.imu]:
+            if sensor is not None and sensor.is_alive:
+                sensor.stop()
+                sensor.destroy()
+        # 销毁车辆
+        if self.vehicle is not None and self.vehicle.is_alive:
             self.vehicle.destroy()
-            self.vehicle = None
-            print("[资源清理] 车辆已销毁")
-            sys.stdout.flush()
-        while not self.image_queue.empty():
-            self.image_queue.get()
\ No newline at end of file
+        # 清空队列
+        for q in [self.image_queue, self.lidar_queue, self.imu_queue]:
+            while not q.empty():
+                q.get()
+        print("[资源清理] 所有传感器和车辆已销毁")
\ No newline at end of file
diff --git a/src/self_driving_car_navigation/perception_module.py b/src/self_driving_car_navigation/perception_module.py
index 27bc8c27bf..fc87b0e058 100644
--- a/src/self_driving_car_navigation/perception_module.py
+++ b/src/self_driving_car_navigation/perception_module.py
@@ -4,42 +4,50 @@
 class PerceptionModule(nn.Module):
     def __init__(self):
         super(PerceptionModule, self).__init__()
-        # 原有语义分割网络保持不变
-        self.segmentation_net = nn.Sequential(
-            nn.Conv2d(3, 16, kernel_size=3, stride=1, padding=1),
+        # 图像特征提取
+        self.image_cnn = nn.Sequential(
+            nn.Conv2d(3, 32, kernel_size=3, stride=2, padding=1),  # (3,128,128)→(32,64,64)
             nn.ReLU(),
-            nn.MaxPool2d(kernel_size=2, stride=2),
-            nn.Conv2d(16, 128, kernel_size=3, stride=1, padding=1),
+            nn.Conv2d(32, 64, kernel_size=3, stride=2, padding=1),  # (32,64,64)→(64,32,32)
             nn.ReLU(),
-            nn.MaxPool2d(kernel_size=2, stride=2),
-            nn.Conv2d(128, 256, kernel_size=3, stride=1, padding=1),
+            nn.Conv2d(64, 128, kernel_size=3, stride=2, padding=1),  # (64,32,32)→(128,16,16)
             nn.ReLU(),
-            nn.MaxPool2d(kernel_size=2, stride=2)
+            nn.Flatten()  # 128*16*16=32768
         )
         
-        # 修正：激光雷达障碍物检测子网络（输入通道从256改为1，适配实际激光雷达通道数）
-        self.obstacle_net = nn.Sequential(
-            nn.Conv1d(in_channels=1, out_channels=128, kernel_size=3, padding=1),  # 输入通道=1（激光雷达数据通道数）
+        # 激光雷达特征提取（360度距离）
+        self.lidar_cnn = nn.Sequential(
+            nn.Conv1d(1, 64, kernel_size=5, padding=2),  # (1,360)→(64,360)
             nn.ReLU(),
-            nn.MaxPool1d(kernel_size=2),
-            nn.Conv1d(in_channels=128, out_channels=64, kernel_size=3, padding=1),
+            nn.MaxPool1d(2),  # (64,360)→(64,180)
+            nn.Conv1d(64, 128, kernel_size=5, padding=2),  # (64,180)→(128,180)
             nn.ReLU(),
-            nn.AdaptiveMaxPool1d(output_size=16),  # 固定输出长度
-            nn.Flatten()  # 展平为特征向量
+            nn.Flatten()  # 128*180=23040
         )
 
     def forward(self, imu_data, image, lidar_data):
-        # 原有视觉特征提取保持不变
-        segmentation = self.segmentation_net(image)
-        scene_info = segmentation.mean(dim=(2, 3))  # 视觉场景信息
-        odometry = imu_data  # IMU里程计数据
-        boundary = lidar_data.max(dim=1)[0]  # 边界检测保持不变
-        
-        # 关键修改：将激光雷达4D数据[batch, 1, 64, 64]转为3D[batch, 1, 64*64]，适配1D卷积
-        # 展平后两个空间维度（64x64 → 4096）
-        lidar_reshaped = lidar_data.flatten(start_dim=2)  # 形状变为[batch, 1, 64*64=4096]
-        
-        # 用专用子网络处理激光雷达数据，提取障碍物特征
-        obstacles = self.obstacle_net(lidar_reshaped)  # 输出[batch, 64*16=1024]
+        # 图像特征
+        image_features = self.image_cnn(image)  # (batch, 32768)
+        
+        # 激光雷达特征
+        lidar_features = self.lidar_cnn(lidar_data)  # (batch, 23040)
+        
+        # 场景信息（融合图像和激光雷达）
+        scene_info = torch.cat([image_features, lidar_features], dim=1)  # (batch, 55808)
+        
+        # 语义分割（简化为图像特征）
+        segmentation = image_features
+        
+        # 里程计（IMU数据）
+        odometry = imu_data
+        
+        # 障碍物特征（激光雷达）
+        obstacles = lidar_features
+        
+        # 边界特征（最大/最小距离）
+        boundary = torch.cat([
+            lidar_data.min(dim=2)[0],
+            lidar_data.max(dim=2)[0]
+        ], dim=1)  # (batch, 2)
         
         return scene_info, segmentation, odometry, obstacles, boundary
\ No newline at end of file
diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py
index 500e117e14..c8830e5eda 100644
--- a/src/self_driving_car_navigation/run_simulation.py
+++ b/src/self_driving_car_navigation/run_simulation.py
@@ -2,34 +2,26 @@
 import time
 import sys
 import math
+import carla
 from models.perception_module import PerceptionModule
 from models.attention_module import CrossDomainAttention
 from models.decision_module import DecisionModule
 from _agent.carla_environment import CarlaEnvironment
-import carla
 
 print("="*60)
 print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}")
 print(f"[Python解释器] {sys.executable}")
-print(f"[虚拟环境] {sys.prefix}")
 print("="*60)
 sys.stdout.flush()
 
 class IntegratedSystem:
     def __init__(self, device='cpu'):
-        print("\n[模型初始化] 开始加载感知、注意力和决策模块...")
-        sys.stdout.flush()
+        print("\n[模型初始化] 加载感知、注意力和决策模块...")
         self.device = device
-        try:
-            self.perception = PerceptionModule().to(self.device)
-            self.attention = CrossDomainAttention(num_blocks=6).to(self.device)
-            self.decision = DecisionModule().to(self.device)
-            print("[模型初始化] 所有模块加载完成")
-            sys.stdout.flush()
-        except Exception as e:
-            print(f"[模型初始化失败] {str(e)}")
-            sys.stdout.flush()
-            raise
+        self.perception = PerceptionModule().to(device)
+        self.attention = CrossDomainAttention(num_blocks=6).to(device)
+        self.decision = DecisionModule().to(device)
+        print("[模型初始化] 完成")
 
     def forward(self, image, lidar_data, imu_data):
         scene_info, segmentation, odometry, obstacles, boundary = self.perception(imu_data, image, lidar_data)
@@ -37,101 +29,125 @@ def forward(self, image, lidar_data, imu_data):
         policy, value = self.decision(fused_features)
         return torch.mean(policy, dim=1), value
 
+def apply_urgent_avoidance(obstacle_distances):
+    """分级避障+逃生策略"""
+    # 1. 前方碰撞危险（≤1.5米）→ 紧急刹车
+    if obstacle_distances['front'] <= 1.5:
+        return (0.0, 0.0, 1.0)
+    
+    # 2. 侧方碰撞危险（≤1.2米）→ 微调避让
+    if obstacle_distances['left'] <= 1.2 or obstacle_distances['right'] <= 1.2:
+        if obstacle_distances['left'] > obstacle_distances['right']:
+            return (0.1, 0.3, 0.0)  # 左微调
+        else:
+            return (0.1, -0.3, 0.0)  # 右微调
+    
+    # 3. 前方近距离（≤3米）→ 转向绕开
+    if obstacle_distances['front'] <= 3.0:
+        if obstacle_distances['left'] > obstacle_distances['right']:
+            return (0.2, 0.4, 0.0)  # 左转向
+        else:
+            return (0.2, -0.4, 0.0)  # 右转向
+    
+    # 4. 后方过近（≤1.5米）→ 后退逃生
+    if obstacle_distances['rear'] <= 1.5:
+        if obstacle_distances['left'] > 2.0:
+            return (-0.2, 0.3, 0.0)  # 左转向+后退
+        elif obstacle_distances['right'] > 2.0:
+            return (-0.2, -0.3, 0.0)  # 右转向+后退
+        else:
+            return (-0.2, 0.0, 0.0)  # 直接后退
+    
+    return None  # 无紧急情况
+
 def run_simulation():
     env = None
     try:
-        print("\n[CARLA连接] 开始创建环境...")
-        sys.stdout.flush()
+        print("\n[CARLA连接] 创建环境...")
         env = CarlaEnvironment()
         
-        print("\n[环境重置] 开始生成车辆和传感器...")
-        sys.stdout.flush()
+        print("\n[环境重置] 生成车辆和传感器...")
         env.reset()
         
         if not env.vehicle or not env.vehicle.is_alive:
-            raise RuntimeError("车辆生成失败！请重启CARLA或更换场景（如Town03）")
+            raise RuntimeError("车辆生成失败，请检查CARLA是否正常运行")
         print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}）")
-        sys.stdout.flush()
 
-        # 获取CARLA可视化镜头控制器
+        # 镜头跟随
         spectator = env.world.get_spectator()
 
+        # 模型设备
         device = 'cuda' if torch.cuda.is_available() else 'cpu'
         print(f"\n[设备信息] 模型运行在 {device} 上")
-        sys.stdout.flush()
         system = IntegratedSystem(device=device)
 
-        # 转向平滑参数
-        steer_buffer = []
-        smooth_window = 3
+        # 控制参数
+        max_forward_throttle = 0.5
+        max_backward_throttle = -0.3
+        max_steer = 0.6
         last_steer = 0.0
-        max_steer_delta = 0.03
+        steer_smooth_factor = 0.2
 
-        print("\n[仿真开始] 共运行100步（镜头固定正后方5m、上方2m，实时跟随）...")
+        print("\n[仿真开始] 共运行100步...")
         sys.stdout.flush()
-        for step in range(100):
-            # 获取摄像头观测
+
+        for step in range(200):
+            # 获取传感器数据SZ
             observation = env.get_observation()
-            if observation is None or observation.size == 0:
-                print(f"[警告] 第{step+1}步未获取到图像数据")
-                sys.stdout.flush()
-
-            # 转换为模型输入格式
-            image = torch.from_numpy(observation.copy()).permute(2, 0, 1).unsqueeze(0).float().to(device) / 255.0
-            lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(device)
-            imu_data = torch.randn(1, 6).to(device)
-
-            # 模型推理
-            policy, _ = system.forward(image, lidar_data, imu_data)
-
-            # 放大油门
-            raw_throttle = float(policy[0][0].clamp(-0.3, 0.8))
-            throttle = max(0.4, min(1.0, raw_throttle * 5))
-
-            # 平滑转向
-            raw_steer = float(policy[0][1].clamp(-0.5, 0.5))
-            steer_buffer.append(raw_steer)
-            if len(steer_buffer) > smooth_window:
-                steer_buffer.pop(0)
-            smooth_steer = sum(steer_buffer) / len(steer_buffer)
-            delta = smooth_steer - last_steer
-            delta = max(-max_steer_delta, min(max_steer_delta, delta))
-            final_steer = last_steer + delta
-            final_steer = max(-1.0, min(1.0, final_steer))
-            last_steer = final_steer
-
-            # 执行控制指令
-            control = carla.VehicleControl(throttle=throttle, steer=final_steer)
+            image = observation['image']
+            lidar_distances = observation['lidar_distances']
+            imu_data = observation['imu']
+
+            # 障碍物检测
+            obstacle_distances = env.get_obstacle_directions(lidar_distances)
+            print(f"\n[障碍物] 前{obstacle_distances['front']:.1f}m | 后{obstacle_distances['rear']:.1f}m | "
+                  f"左{obstacle_distances['left']:.1f}m | 右{obstacle_distances['right']:.1f}m")
+            sys.stdout.flush()
+
+            # 避障策略
+            avoid_action = apply_urgent_avoidance(obstacle_distances)
+            if avoid_action is not None:
+                throttle, steer, brake = avoid_action
+                print(f"[策略执行] 油门={throttle:.2f}, 转向={steer:.2f}, 刹车={brake:.2f}")
+            else:
+                # 模型控制
+                image_tensor = torch.from_numpy(image).permute(2, 0, 1).unsqueeze(0).float().to(device) / 255.0
+                lidar_tensor = torch.from_numpy(lidar_distances).unsqueeze(0).unsqueeze(0).float().to(device)
+                imu_tensor = torch.from_numpy(imu_data).unsqueeze(0).float().to(device)
+
+                with torch.no_grad():
+                    policy, _ = system.forward(image_tensor, lidar_tensor, imu_tensor)
+
+                throttle = float(policy[0][0].clamp(max_backward_throttle, max_forward_throttle))
+                raw_steer = float(policy[0][1].clamp(-max_steer, max_steer))
+                brake = 0.0
+
+                # 平滑转向
+                steer = last_steer * (1 - steer_smooth_factor) + raw_steer * steer_smooth_factor
+                last_steer = steer
+
+            # 执行控制
+            control = carla.VehicleControl(
+                throttle=throttle,
+                steer=steer,
+                brake=brake,
+                hand_brake=False
+            )
             env.vehicle.apply_control(control)
 
-            # ========== 修复API错误：手动计算镜头朝向（严格正后方+看向车辆） ==========
+            # 镜头跟随设置
             vehicle_transform = env.vehicle.get_transform()
-            # 1. 车辆局部坐标系转世界坐标系（严格正后方5m、上方2m）
-            local_cam_loc = carla.Location(x=-5.0, y=0.0, z=2.0)  # 车辆自身正后方
-            camera_location = vehicle_transform.transform(local_cam_loc)
-            
-            # 2. 手动计算镜头朝向（看向车辆）
-            # 计算镜头到车辆的方向向量
-            dir_x = vehicle_transform.location.x - camera_location.x
-            dir_y = vehicle_transform.location.y - camera_location.y
-            dir_z = vehicle_transform.location.z - camera_location.z
-            # 计算水平旋转角yaw（绕z轴）
+            cam_loc = vehicle_transform.transform(carla.Location(x=-5.0, z=2.0))
+            dir_x = vehicle_transform.location.x - cam_loc.x
+            dir_y = vehicle_transform.location.y - cam_loc.y
             yaw = math.atan2(dir_y, dir_x) * 180 / math.pi
-            # 计算俯仰角pitch（绕y轴，轻微俯视）
-            pitch = -10.0  # 固定俯视10度，确保看到车辆
-            # 构建旋转对象
-            camera_rotation = carla.Rotation(pitch=pitch, yaw=yaw, roll=0.0)
-            
-            # 3. 应用镜头设置
-            spectator.set_transform(carla.Transform(camera_location, camera_rotation))
-            # ======================================================================
-
-            # 打印控制参数
-            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {final_steer:.2f}")
+            spectator.set_transform(carla.Transform(cam_loc, carla.Rotation(pitch=-10, yaw=yaw)))
+
+            print(f"[步骤 {step+1}/00] 油门: {throttle:.2f} | 转向: {steer:.2f} | 刹车: {brake:.2f}")
             sys.stdout.flush()
             time.sleep(0.1)
 
-        print("\n[仿真结束] 已完成100步运行")
+        print("\n[仿真结束]")
         sys.stdout.flush()
 
     except Exception as e:
@@ -139,11 +155,9 @@ def run_simulation():
         sys.stdout.flush()
     finally:
         if env is not None:
-            print("\n[资源清理] 正在销毁车辆和传感器...")
-            sys.stdout.flush()
+            print("\n[资源清理] 销毁资源...")
             env.close()
-        print("\n[程序退出] 所有操作已完成")
-        sys.stdout.flush()
+        print("\n[程序退出]")
 
 if __name__ == "__main__":
     run_simulation()
\ No newline at end of file

From e70d6385acadaed53deb27c09e6feec8d2847b55 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Wed, 10 Dec 2025 22:35:51 +0800
Subject: [PATCH 15/19] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=BD=A6=E8=BE=86?=
 =?UTF-8?q?=E7=94=9F=E6=88=90=E4=BD=8D=E7=BD=AE=E5=9B=BA=E5=AE=9A=E9=97=AE?=
 =?UTF-8?q?=E9=A2=98=EF=BC=8C=E6=96=B0=E5=A2=9E=E9=9A=8F=E6=9C=BA=E9=80=89?=
 =?UTF-8?q?=E7=82=B9=E7=AD=96=E7=95=A5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 74 ++++++++++---------
 1 file changed, 41 insertions(+), 33 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index f98c309b37..b5c7c0d3e7 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -1,9 +1,10 @@
 import gym
 import carla
 import numpy as np
-import sys
 import time
+import sys
 from queue import Queue
+from gym import spaces
 
 class CarlaEnvironment(gym.Env):
     def __init__(self):
@@ -56,12 +57,9 @@ def process_image(self, image):
         """处理摄像头数据，转换为RGB格式"""
         try:
             array = np.frombuffer(image.raw_data, dtype=np.uint8)
-
-            array = np.reshape(array, (image.height, image.width, 4))
+            array = np.reshape(array, (image.height, image.width, 4))  # (H,W,4)
             array = array[:, :, :3]  # 移除alpha通道
-            array = array[:, :, ::-1]  # BGR转RGB
-            array = array.copy()  # 消除负步长
-
+            array = array[:, :, ::-1].copy()  # BGR转RGB
             if self.image_queue.full():
                 self.image_queue.get()
             self.image_queue.put(array)
@@ -110,48 +108,51 @@ def reset(self):
         time.sleep(0.5)
         self._spawn_vehicle()
         if self.vehicle:
-
-            self._spawn_camera()
-
+            self._spawn_sensors()
         time.sleep(1.0)  # 等待传感器就绪
         return self.get_observation()
 
     def _spawn_vehicle(self):
-
-        # 选择稳定车型（特斯拉Model3）
+        """生成车辆（特斯拉Model3）- 优化生成点选择逻辑"""
+        import random  # 仅在此方法内导入随机模块
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
-        vehicle_bp.set_attribute('color', '255,0,0')  # 红色，便于观察
-        vehicle_bp.set_attribute('role_name', 'drone')
-
-        # 关键调整：使用第10个生成点（通常在主路中央，避免障碍物）
-        spawn_index = 10  # 可根据场景调整（0~264）
+        vehicle_bp.set_attribute('color', '255,0,0')  # 红色
+        vehicle_bp.set_attribute('role_name', 'ego_vehicle')
+
+        # 核心优化：随机选择生成点（解决固定位置问题）
+        if self.spawn_points:
+            # 随机打乱生成点顺序
+            random.shuffle(self.spawn_points)
+            # 尝试前5个随机生成点（避免位置被占用）
+            for spawn_point in self.spawn_points[:5]:
+                self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
+                if self.vehicle:
+                    self.vehicle.set_autopilot(False)
+                    self.vehicle.set_simulate_physics(True)
+                    print(f"[车辆生成] 成功在随机位置生成（ID: {self.vehicle.id}）")
+                    return
+        
+        # 若随机位置失败，fallback到原逻辑（确保兼容性）
+        spawn_index = 10
         for i in range(3):
-            # 优先用指定生成点，失败则重试
             spawn_point = self.spawn_points[(spawn_index + i) % len(self.spawn_points)]
-            print(f"[车辆生成] 尝试在生成点 {spawn_index + i} 生成车辆（主路中央）...")
-            sys.stdout.flush()
             self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
             if self.vehicle:
                 self.vehicle.set_autopilot(False)
-                self.vehicle.set_simulate_physics(True)  # 强制启用物理引擎
-                print(f"[车辆生成] 成功（ID: {self.vehicle.id}）- 位置：主路中央")
-                sys.stdout.flush()
-
+                self.vehicle.set_simulate_physics(True)
+                print(f"[车辆生成] 随机位置失败，使用备用位置（ID: {self.vehicle.id}）")
                 return
+        
         raise RuntimeError("车辆生成失败，请重启CARLA或更换场景（如Town03）")
 
     def _spawn_sensors(self):
-        """生成摄像头、激光雷达、IMU传感器（核心修正：兼容激光雷达参数）"""
+        """生成摄像头、激光雷达、IMU传感器（兼容激光雷达参数）"""
         # 1. 前视摄像头
         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.05')
-
-        # 摄像头位置：车辆前方1.5米，高度2.4米（驾驶员视角）
-        camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
-
         self.camera = self.world.spawn_actor(
             camera_bp, carla.Transform(carla.Location(x=1.5, z=2.4)), attach_to=self.vehicle
         )
@@ -208,11 +209,18 @@ def get_obstacle_directions(self, lidar_distances):
 
     def step(self, action):
         """执行动作并返回环境反馈"""
-        control = carla.VehicleControl(
-            throttle=float(action[0]),
-            steer=float(action[1]),
-            brake=float(action[2])
-        )
+        # 适配离散动作到连续控制（根据原始测试代码的动作定义）
+        if action == 0:  # 前进
+            control = carla.VehicleControl(throttle=0.5, steer=0.0, brake=0.0)
+        elif action == 1:  # 左转
+            control = carla.VehicleControl(throttle=0.3, steer=0.5, brake=0.0)
+        elif action == 2:  # 右转
+            control = carla.VehicleControl(throttle=0.3, steer=-0.5, brake=0.0)
+        elif action == 3:  # 后退
+            control = carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0, reverse=True)
+        else:  # 默认空动作
+            control = carla.VehicleControl(throttle=0.0, steer=0.0, brake=1.0)
+
         self.vehicle.apply_control(control)
         observation = self.get_observation()
         reward = 1.0  # 基础存活奖励

From 955d3aedaa9e45f8c251de13554457bcc79b10d6 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Fri, 12 Dec 2025 00:20:32 +0800
Subject: [PATCH 16/19] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=B0=8F=E8=BD=A6?=
 =?UTF-8?q?=E5=81=8F=E7=A6=BB=E8=BD=A6=E9=81=93/=E9=A9=B6=E5=85=A5?=
 =?UTF-8?q?=E8=8D=89=E5=9C=B0=E9=97=AE=E9=A2=98=EF=BC=8C=E9=80=82=E9=85=8D?=
 =?UTF-8?q?CARLA=200.9.11=E5=B9=B6=E4=BC=98=E5=8C=96=E6=8E=A7=E5=88=B6?=
 =?UTF-8?q?=E9=80=BB=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 125 ++++++------
 .../run_simulation.py                         | 184 ++++++------------
 2 files changed, 123 insertions(+), 186 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index b5c7c0d3e7..e50a7bf630 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -12,6 +12,7 @@ def __init__(self):
         self.client = None
         self.world = None
         self.blueprint_library = None
+        self.settings = None  # 用于保存世界设置
         self._connect_carla()
 
         # 观测空间定义
@@ -36,16 +37,23 @@ def __init__(self):
         sys.stdout.flush()
 
     def _connect_carla(self):
-        """连接CARLA服务器，支持重试"""
+        """连接CARLA服务器，支持重试并启用同步模式"""
         retry_count = 3
         for i in range(retry_count):
             try:
                 print(f"[CARLA连接] 尝试第{i+1}次连接（localhost:2000）...")
                 self.client = carla.Client('localhost', 2000)
-                self.client.set_timeout(15.0)  # 超时时间15秒
+                self.client.set_timeout(15.0)
                 self.world = self.client.get_world()
                 self.blueprint_library = self.world.get_blueprint_library()
-                print("[CARLA连接] 成功连接到模拟器")
+                
+                # 启用同步模式（关键优化：解决数据不同步问题）
+                self.settings = self.world.get_settings()
+                self.settings.synchronous_mode = True
+                self.settings.fixed_delta_seconds = 1/30  # 固定30帧
+                self.world.apply_settings(self.settings)
+                
+                print("[CARLA连接] 成功连接到模拟器并启用同步模式")
                 return
             except Exception as e:
                 print(f"[CARLA连接失败] {str(e)}")
@@ -57,8 +65,8 @@ def process_image(self, image):
         """处理摄像头数据，转换为RGB格式"""
         try:
             array = np.frombuffer(image.raw_data, dtype=np.uint8)
-            array = np.reshape(array, (image.height, image.width, 4))  # (H,W,4)
-            array = array[:, :, :3]  # 移除alpha通道
+            array = np.reshape(array, (image.height, image.width, 4))
+            array = array[:, :, :3]
             array = array[:, :, ::-1].copy()  # BGR转RGB
             if self.image_queue.full():
                 self.image_queue.get()
@@ -69,15 +77,12 @@ def process_image(self, image):
     def process_lidar(self, data):
         """处理激光雷达数据，生成360度距离数组"""
         try:
-            # 解析点云数据 (x,y,z,intensity)
             points = np.frombuffer(data.raw_data, dtype=np.dtype('f4')).reshape(-1, 4)[:, :3]
-            distances = np.linalg.norm(points, axis=1)  # 计算每个点到车辆的距离
-            angles = np.arctan2(points[:, 1], points[:, 0]) * 180 / np.pi  # 计算角度（度）
-            angles = (angles + 360) % 360  # 归一化到0-360度
+            distances = np.linalg.norm(points, axis=1)
+            angles = np.arctan2(points[:, 1], points[:, 0]) * 180 / np.pi
+            angles = (angles + 360) % 360
 
-            # 初始化360度距离数组（默认50米）
             lidar_distances = np.full(360, 50.0, dtype=np.float32)
-            # 填充每个角度的最近距离
             for angle, dist in zip(angles, distances):
                 angle_idx = int(round(angle)) % 360
                 if dist < lidar_distances[angle_idx]:
@@ -103,36 +108,37 @@ def process_imu(self, data):
             print(f"[IMU处理错误] {str(e)}")
 
     def reset(self):
-        """重置环境，生成车辆和传感器"""
+        """重置环境，生成车辆和传感器（启用CARLA原生自动驾驶）"""
         self.close()
         time.sleep(0.5)
         self._spawn_vehicle()
         if self.vehicle:
             self._spawn_sensors()
-        time.sleep(1.0)  # 等待传感器就绪
+            # 启用CARLA原生自动驾驶（关键优化：使用成熟的车道保持逻辑）
+            self.vehicle.set_autopilot(True)
+        time.sleep(1.0)
         return self.get_observation()
 
     def _spawn_vehicle(self):
-        """生成车辆（特斯拉Model3）- 优化生成点选择逻辑"""
-        import random  # 仅在此方法内导入随机模块
+        """生成车辆（特斯拉Model3）- 选择车道内的生成点"""
+        import random
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
-        vehicle_bp.set_attribute('color', '255,0,0')  # 红色
+        vehicle_bp.set_attribute('color', '255,0,0')
         vehicle_bp.set_attribute('role_name', 'ego_vehicle')
 
-        # 核心优化：随机选择生成点（解决固定位置问题）
+        # 优先选择车道内的生成点（减少初始偏离）
         if self.spawn_points:
-            # 随机打乱生成点顺序
             random.shuffle(self.spawn_points)
-            # 尝试前5个随机生成点（避免位置被占用）
-            for spawn_point in self.spawn_points[:5]:
+            # 尝试前10个生成点，确保车辆在道路上
+            for spawn_point in self.spawn_points[:10]:
                 self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
                 if self.vehicle:
                     self.vehicle.set_autopilot(False)
                     self.vehicle.set_simulate_physics(True)
-                    print(f"[车辆生成] 成功在随机位置生成（ID: {self.vehicle.id}）")
+                    print(f"[车辆生成] 成功在道路生成点生成（ID: {self.vehicle.id}）")
                     return
         
-        # 若随机位置失败，fallback到原逻辑（确保兼容性）
+        # 备用生成逻辑
         spawn_index = 10
         for i in range(3):
             spawn_point = self.spawn_points[(spawn_index + i) % len(self.spawn_points)]
@@ -140,47 +146,47 @@ def _spawn_vehicle(self):
             if self.vehicle:
                 self.vehicle.set_autopilot(False)
                 self.vehicle.set_simulate_physics(True)
-                print(f"[车辆生成] 随机位置失败，使用备用位置（ID: {self.vehicle.id}）")
+                print(f"[车辆生成] 使用备用位置（ID: {self.vehicle.id}）")
                 return
         
-        raise RuntimeError("车辆生成失败，请重启CARLA或更换场景（如Town03）")
+        raise RuntimeError("车辆生成失败，请重启CARLA或更换场景")
 
     def _spawn_sensors(self):
-        """生成摄像头、激光雷达、IMU传感器（兼容激光雷达参数）"""
-        # 1. 前视摄像头
+        """生成传感器（适配0.9.11版本参数，修正激光雷达垂直视野）"""
+        # 前视摄像头（优化视角，更贴近驾驶视角）
         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.05')
+        camera_bp.set_attribute('fov', '100')  # 扩大视野
+        camera_bp.set_attribute('sensor_tick', '0.033')  # 30Hz
         self.camera = self.world.spawn_actor(
-            camera_bp, carla.Transform(carla.Location(x=1.5, z=2.4)), attach_to=self.vehicle
+            camera_bp, carla.Transform(carla.Location(x=2.0, z=1.5)), attach_to=self.vehicle
         )
         self.camera.listen(self.process_image)
 
-        # 2. 激光雷达（关键修正：用upper_fov和lower_fov替代vertical_fov）
+        # 激光雷达（0.9.11兼容参数：用upper_fov和lower_fov替代vertical_fov）
         lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast')
-        lidar_bp.set_attribute('channels', '64')  # 64线
-        lidar_bp.set_attribute('range', '50')  # 最大50米
-        lidar_bp.set_attribute('points_per_second', '200000')
-        lidar_bp.set_attribute('rotation_frequency', '20')  # 20Hz
-        lidar_bp.set_attribute('horizontal_fov', '360')  # 全向扫描
-        # 垂直角度范围：-30°到30°（兼容所有版本）
-        lidar_bp.set_attribute('upper_fov', '30.0')    # 上角度
-        lidar_bp.set_attribute('lower_fov', '-30.0')   # 下角度
+        lidar_bp.set_attribute('channels', '32')
+        lidar_bp.set_attribute('range', '50')
+        lidar_bp.set_attribute('points_per_second', '100000')
+        lidar_bp.set_attribute('rotation_frequency', '10')
+        lidar_bp.set_attribute('horizontal_fov', '360')
+        # 0.9.11专用：垂直视野通过上下视角差定义（15 - (-15) = 30度）
+        lidar_bp.set_attribute('upper_fov', '15.0')   # 上视角
+        lidar_bp.set_attribute('lower_fov', '-15.0')  # 下视角
         self.lidar = self.world.spawn_actor(
             lidar_bp, carla.Transform(carla.Location(x=0.0, z=2.0)), attach_to=self.vehicle
         )
         self.lidar.listen(self.process_lidar)
 
-        # 3. IMU传感器
+        # IMU传感器
         imu_bp = self.blueprint_library.find('sensor.other.imu')
-        imu_bp.set_attribute('sensor_tick', '0.05')
+        imu_bp.set_attribute('sensor_tick', '0.033')
         self.imu = self.world.spawn_actor(
             imu_bp, carla.Transform(), attach_to=self.vehicle
         )
         self.imu.listen(self.process_imu)
-        print("[传感器] 全向激光雷达+摄像头+IMU初始化成功")
+        print("[传感器] 初始化成功")
 
     def get_observation(self):
         """获取传感器数据（确保数据就绪）"""
@@ -193,12 +199,11 @@ def get_observation(self):
         }
 
     def get_obstacle_directions(self, lidar_distances):
-        """计算前/后/左/右四个方向的最近障碍物距离"""
-        # 角度范围定义（度）
-        front_angles = np.concatenate([np.arange(345, 360), np.arange(0, 16)])  # 前方：-15~15°
-        rear_angles = np.arange(165, 196)  # 后方：165~195°
-        left_angles = np.arange(75, 106)  # 左方：75~105°
-        right_angles = np.arange(255, 286)  # 右方：255~285°（-105~-75°）
+        """计算四个方向的最近障碍物距离"""
+        front_angles = np.concatenate([np.arange(345, 360), np.arange(0, 16)])
+        rear_angles = np.arange(165, 196)
+        left_angles = np.arange(75, 106)
+        right_angles = np.arange(255, 286)
 
         return {
             'front': np.min(lidar_distances[front_angles]),
@@ -207,28 +212,16 @@ def get_obstacle_directions(self, lidar_distances):
             'right': np.min(lidar_distances[right_angles])
         }
 
-    def step(self, action):
-        """执行动作并返回环境反馈"""
-        # 适配离散动作到连续控制（根据原始测试代码的动作定义）
-        if action == 0:  # 前进
-            control = carla.VehicleControl(throttle=0.5, steer=0.0, brake=0.0)
-        elif action == 1:  # 左转
-            control = carla.VehicleControl(throttle=0.3, steer=0.5, brake=0.0)
-        elif action == 2:  # 右转
-            control = carla.VehicleControl(throttle=0.3, steer=-0.5, brake=0.0)
-        elif action == 3:  # 后退
-            control = carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0, reverse=True)
-        else:  # 默认空动作
-            control = carla.VehicleControl(throttle=0.0, steer=0.0, brake=1.0)
-
-        self.vehicle.apply_control(control)
+    def step(self, action=None):
+        """执行动作（使用自动驾驶时可忽略action）"""
+        # 当启用autopilot时，不需要手动控制
         observation = self.get_observation()
-        reward = 1.0  # 基础存活奖励
+        reward = 1.0
         done = False
         return observation, reward, done, {}
 
     def close(self):
-        """清理资源（传感器和车辆）"""
+        """清理资源并恢复世界设置"""
         # 销毁传感器
         for sensor in [self.camera, self.lidar, self.imu]:
             if sensor is not None and sensor.is_alive:
@@ -241,4 +234,8 @@ def close(self):
         for q in [self.image_queue, self.lidar_queue, self.imu_queue]:
             while not q.empty():
                 q.get()
+        # 恢复世界设置
+        if self.settings:
+            self.settings.synchronous_mode = False
+            self.world.apply_settings(self.settings)
         print("[资源清理] 所有传感器和车辆已销毁")
\ No newline at end of file
diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py
index e5896ebe19..36ad14e01c 100644
--- a/src/self_driving_car_navigation/run_simulation.py
+++ b/src/self_driving_car_navigation/run_simulation.py
@@ -1,14 +1,10 @@
 import torch
 import time
 import sys
-import math
-
+import pygame
 import carla
 
-
-from models.perception_module import PerceptionModule
-from models.attention_module import CrossDomainAttention
-from models.decision_module import DecisionModule
+# 修正导入路径：适配_agent子目录下的carla_environment.py
 from _agent.carla_environment import CarlaEnvironment
 
 print("="*60)
@@ -17,51 +13,37 @@
 print("="*60)
 sys.stdout.flush()
 
-class IntegratedSystem:
-    def __init__(self, device='cpu'):
-        print("\n[模型初始化] 加载感知、注意力和决策模块...")
-        self.device = device
-        self.perception = PerceptionModule().to(device)
-        self.attention = CrossDomainAttention(num_blocks=6).to(device)
-        self.decision = DecisionModule().to(device)
-        print("[模型初始化] 完成")
-
-    def forward(self, image, lidar_data, imu_data):
-        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 torch.mean(policy, dim=1), value
-
-def apply_urgent_avoidance(obstacle_distances):
-    """分级避障+逃生策略"""
-    # 1. 前方碰撞危险（≤1.5米）→ 紧急刹车
-    if obstacle_distances['front'] <= 1.5:
-        return (0.0, 0.0, 1.0)
+def set_spectator_smooth(world, vehicle, last_transform=None):
+    """平滑跟随视角（移植自参考代码的核心逻辑）"""
+    spectator = world.get_spectator()
+    vehicle_tf = vehicle.get_transform()
+    # 目标视角：车辆后上方，提供良好视野
+    target_tf = carla.Transform(
+        vehicle_tf.transform(carla.Location(x=-8, z=3, y=0.5)),
+        vehicle_tf.rotation
+    )
     
-    # 2. 侧方碰撞危险（≤1.2米）→ 微调避让
-    if obstacle_distances['left'] <= 1.2 or obstacle_distances['right'] <= 1.2:
-        if obstacle_distances['left'] > obstacle_distances['right']:
-            return (0.1, 0.3, 0.0)  # 左微调
-        else:
-            return (0.1, -0.3, 0.0)  # 右微调
+    if last_transform is None:
+        spectator.set_transform(target_tf)
+        return target_tf
     
-    # 3. 前方近距离（≤3米）→ 转向绕开
-    if obstacle_distances['front'] <= 3.0:
-        if obstacle_distances['left'] > obstacle_distances['right']:
-            return (0.2, 0.4, 0.0)  # 左转向
-        else:
-            return (0.2, -0.4, 0.0)  # 右转向
+    # 线性插值平滑过渡
+    def lerp(a, b, t):
+        return a + t * (b - a)
     
-    # 4. 后方过近（≤1.5米）→ 后退逃生
-    if obstacle_distances['rear'] <= 1.5:
-        if obstacle_distances['left'] > 2.0:
-            return (-0.2, 0.3, 0.0)  # 左转向+后退
-        elif obstacle_distances['right'] > 2.0:
-            return (-0.2, -0.3, 0.0)  # 右转向+后退
-        else:
-            return (-0.2, 0.0, 0.0)  # 直接后退
-    
-    return None  # 无紧急情况
+    smooth_loc = carla.Location(
+        x=lerp(last_transform.location.x, target_tf.location.x, 0.1),
+        y=lerp(last_transform.location.y, target_tf.location.y, 0.1),
+        z=lerp(last_transform.location.z, target_tf.location.z, 0.1)
+    )
+    smooth_rot = carla.Rotation(
+        pitch=lerp(last_transform.rotation.pitch, target_tf.rotation.pitch, 0.1),
+        yaw=lerp(last_transform.rotation.yaw, target_tf.rotation.yaw, 0.1),
+        roll=lerp(last_transform.rotation.roll, target_tf.rotation.roll, 0.1)
+    )
+    smooth_tf = carla.Transform(smooth_loc, smooth_rot)
+    spectator.set_transform(smooth_tf)
+    return smooth_tf
 
 def run_simulation():
     env = None
@@ -74,85 +56,43 @@ def run_simulation():
         
         if not env.vehicle or not env.vehicle.is_alive:
             raise RuntimeError("车辆生成失败，请检查CARLA是否正常运行")
-        print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}）")
-
-
-        # 获取CARLA可视化镜头控制器
-        spectator = env.world.get_spectator()
+        print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}），已启用自动驾驶")
 
+        # 初始化平滑视角
+        last_spectator_tf = set_spectator_smooth(env.world, env.vehicle)
+        print("视角已切换至车辆后上方（平滑跟随模式）")
 
-        device = 'cuda' if torch.cuda.is_available() else 'cpu'
-        print(f"\n[设备信息] 模型运行在 {device} 上")
-        system = IntegratedSystem(device=device)
-
-
-        # 新增：转向平滑参数（减少晃动）
-        steer_buffer = []  # 存储最近3步转向值
-        smooth_window = 3  # 滑动平均窗口
-        last_steer = 0.0  # 上一步转向值
-        max_steer_delta = 0.03  # 转向最大变化量（限制高频波动）
-
-        print("\n[仿真开始] 共运行100步（车辆将明显移动）...")
+        # 初始化时钟控制帧率
+        clock = pygame.time.Clock()
 
+        print("\n[仿真开始] 车辆将沿车道行驶，按Ctrl+C退出...")
         sys.stdout.flush()
-        for step in range(100):
-            # 获取摄像头观测
-
+        
+        # 持续运行仿真（不限制步数）
+        step = 0
+        while True:
+            # 同步CARLA帧（关键优化：保证控制时序稳定）
+            env.world.tick()
+            
+            # 获取观测和障碍物信息
             observation = env.get_observation()
-            image = observation['image']
-            lidar_distances = observation['lidar_distances']
-            imu_data = observation['imu']
-
-            # 障碍物检测
-            obstacle_distances = env.get_obstacle_directions(lidar_distances)
-            print(f"\n[障碍物] 前{obstacle_distances['front']:.1f}m | 后{obstacle_distances['rear']:.1f}m | "
-                  f"左{obstacle_distances['left']:.1f}m | 右{obstacle_distances['right']:.1f}m")
-            sys.stdout.flush()
-
-
-            # 转换为模型输入格式
-            image = torch.from_numpy(observation.copy()).permute(2, 0, 1).unsqueeze(0).float().to(device) / 255.0
-            lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(device)
-            imu_data = torch.randn(1, 6).to(device)
-
-            # 模型推理
-            policy, _ = system.forward(image, lidar_data, imu_data)
-
-
-            # 关键调整1：放大油门值（确保车辆明显移动，最低0.4）
-            raw_throttle = float(policy[0][0].clamp(-0.3, 0.8))  # 原始油门
-            throttle = max(0.4, min(1.0, raw_throttle * 5))  # 放大5倍，最低0.4
-
-            # 关键调整2：平滑转向（减少轮胎晃动）
-            raw_steer = float(policy[0][1].clamp(-0.5, 0.5))  # 原始转向
-            # 滑动平均+限制变化速率
-            steer_buffer.append(raw_steer)
-            if len(steer_buffer) > smooth_window:
-                steer_buffer.pop(0)
-            smooth_steer = sum(steer_buffer) / len(steer_buffer)  # 平均
-            # 限制与上一步的差值
-            delta = smooth_steer - last_steer
-            delta = max(-max_steer_delta, min(max_steer_delta, delta))
-            final_steer = last_steer + delta
-            final_steer = max(-1.0, min(1.0, final_steer))  # 最终限制
-            last_steer = final_steer  # 更新历史值
-
-
-            # 执行控制指令
-            control = carla.VehicleControl(throttle=throttle, steer=final_steer)
-            env.vehicle.apply_control(control)
-
-
-            # 打印详细控制参数
-
-            print(f"[步骤 {step+1}/100] 油门: {throttle:.2f} | 转向: {final_steer:.2f}")
-
-            sys.stdout.flush()
-            time.sleep(0.1)  # 控制仿真速度
-
-        print("\n[仿真结束]")
-        sys.stdout.flush()
-
+            obstacle_distances = env.get_obstacle_directions(observation['lidar_distances'])
+            
+            # 打印状态信息
+            if step % 10 == 0:  # 每10步打印一次
+                print(f"\n[步骤 {step}] 障碍物距离 - 前{obstacle_distances['front']:.1f}m | 后{obstacle_distances['rear']:.1f}m | "
+                      f"左{obstacle_distances['left']:.1f}m | 右{obstacle_distances['right']:.1f}m")
+                sys.stdout.flush()
+            
+            # 更新平滑视角
+            last_spectator_tf = set_spectator_smooth(env.world, env.vehicle, last_spectator_tf)
+            
+            # 控制帧率为30FPS
+            clock.tick(30)
+            step += 1
+
+    except KeyboardInterrupt:
+        print("\n[用户终止] 收到退出信号")
     except Exception as e:
         print(f"\n[仿真错误] {str(e)}")
         sys.stdout.flush()

From 23b0472ca00824670fc62a89bdc3c33455f3cb3d Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Fri, 12 Dec 2025 15:49:37 +0800
Subject: [PATCH 17/19] =?UTF-8?q?=E6=B8=85=E9=99=A4=20CARLA=20=E5=9C=B0?=
 =?UTF-8?q?=E5=9B=BE=E9=BB=98=E8=AE=A4=E5=9C=A8=E8=BD=A6=E9=81=93=E4=B8=8A?=
 =?UTF-8?q?=E7=9A=84=E9=9D=99=E6=80=81=E8=BD=A6=E8=BE=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/self_driving_car_navigation/carla_environment.py | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index e50a7bf630..017c316150 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -47,6 +47,13 @@ def _connect_carla(self):
                 self.world = self.client.get_world()
                 self.blueprint_library = self.world.get_blueprint_library()
                 
+                # 关键修改：清除地图中所有默认静态车辆
+                actors = self.world.get_actors()
+                for actor in actors:
+                    if actor.type_id.startswith('vehicle.'):  # 筛选所有车辆类型
+                        actor.destroy()
+                        print(f"[清除默认车辆] 销毁静态车辆（ID: {actor.id}）")
+                
                 # 启用同步模式（关键优化：解决数据不同步问题）
                 self.settings = self.world.get_settings()
                 self.settings.synchronous_mode = True

From 124def84f8eae6149d33451f1a7deed8939c821d Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Fri, 12 Dec 2025 22:40:34 +0800
Subject: [PATCH 18/19] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E5=8A=A8?=
 =?UTF-8?q?=E9=A9=BE=E9=A9=B6=20NPC=20=E8=BD=A6=E8=BE=86=EF=BC=8C=E6=8F=90?=
 =?UTF-8?q?=E5=8D=87=E8=87=AA=E5=8A=A8=E9=A9=BE=E9=A9=B6=E5=9C=BA=E6=99=AF?=
 =?UTF-8?q?=E9=9A=BE=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 90 +++++++++++++++----
 1 file changed, 71 insertions(+), 19 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index 017c316150..34d1a320b7 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -3,6 +3,7 @@
 import numpy as np
 import time
 import sys
+import random
 from queue import Queue
 from gym import spaces
 
@@ -22,11 +23,12 @@ def __init__(self):
             'imu': gym.spaces.Box(low=-10, high=10, shape=(6,), dtype=np.float32)
         })
 
-        # 传感器和车辆实例
+        # 传感器、车辆和NPC实例
         self.vehicle = None
         self.camera = None
         self.lidar = None
         self.imu = None
+        self.npc_vehicles = []  # 存储NPC车辆实例
         # 数据队列
         self.image_queue = Queue(maxsize=1)
         self.lidar_queue = Queue(maxsize=1)
@@ -47,7 +49,7 @@ def _connect_carla(self):
                 self.world = self.client.get_world()
                 self.blueprint_library = self.world.get_blueprint_library()
                 
-                # 关键修改：清除地图中所有默认静态车辆
+                # 清除地图中所有默认静态车辆
                 actors = self.world.get_actors()
                 for actor in actors:
                     if actor.type_id.startswith('vehicle.'):  # 筛选所有车辆类型
@@ -115,20 +117,24 @@ def process_imu(self, data):
             print(f"[IMU处理错误] {str(e)}")
 
     def reset(self):
-        """重置环境，生成车辆和传感器（启用CARLA原生自动驾驶）"""
+        """重置环境，生成车辆、传感器和NPC（启用自动驾驶）"""
         self.close()
         time.sleep(0.5)
         self._spawn_vehicle()
         if self.vehicle:
             self._spawn_sensors()
-            # 启用CARLA原生自动驾驶（关键优化：使用成熟的车道保持逻辑）
+            # 启用主角车自动驾驶
             self.vehicle.set_autopilot(True)
-        time.sleep(1.0)
+            # 生成NPC车辆（20辆）
+            self._spawn_npcs(20)
+        # 额外同步确保所有车辆启动
+        for _ in range(2):
+            self.world.tick()
+            time.sleep(0.5)
         return self.get_observation()
 
     def _spawn_vehicle(self):
-        """生成车辆（特斯拉Model3）- 选择车道内的生成点"""
-        import random
+        """生成主角车辆（特斯拉Model3）- 选择车道内的生成点"""
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
         vehicle_bp.set_attribute('color', '255,0,0')
         vehicle_bp.set_attribute('role_name', 'ego_vehicle')
@@ -158,29 +164,71 @@ def _spawn_vehicle(self):
         
         raise RuntimeError("车辆生成失败，请重启CARLA或更换场景")
 
+    def _spawn_npcs(self, count):
+        """生成指定数量的NPC车辆并激活自动驾驶"""
+        if not self.spawn_points:
+            print("[NPC生成] 没有可用的生成点")
+            return
+
+        # 筛选可用车辆蓝图（排除主角车）
+        vehicle_blueprints = self.blueprint_library.filter('vehicle')
+        vehicle_blueprints = [bp for bp in vehicle_blueprints if bp.id != 'vehicle.tesla.model3']
+        if not vehicle_blueprints:
+            print("[NPC生成] 没有可用的NPC蓝图")
+            return
+
+        print(f"[NPC生成] 开始生成{count}辆NPC车辆...")
+        spawned_count = 0
+        used_spawn_points = []  # 避免生成点重叠
+
+        # 循环尝试生成
+        while spawned_count < count and len(used_spawn_points) < len(self.spawn_points):
+            # 随机选择未使用的生成点
+            spawn_point = random.choice([p for p in self.spawn_points if p not in used_spawn_points])
+            used_spawn_points.append(spawn_point)
+            
+            # 随机选择车辆蓝图
+            npc_bp = random.choice(vehicle_blueprints)
+            
+            # 尝试生成NPC
+            npc = self.world.try_spawn_actor(npc_bp, spawn_point)
+            if npc:
+                self.npc_vehicles.append(npc)
+                # 启用自动驾驶
+                npc.set_autopilot(True)
+                spawned_count += 1
+                
+                # 每生成5辆同步一次
+                if spawned_count % 5 == 0:
+                    self.world.tick()
+                    time.sleep(0.1)
+
+        # 生成完成后同步
+        self.world.tick()
+        print(f"[NPC生成] 完成，实际生成{spawned_count}辆NPC")
+
     def _spawn_sensors(self):
-        """生成传感器（适配0.9.11版本参数，修正激光雷达垂直视野）"""
-        # 前视摄像头（优化视角，更贴近驾驶视角）
+        """生成传感器（适配0.9.11版本参数）"""
+        # 前视摄像头
         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', '100')  # 扩大视野
-        camera_bp.set_attribute('sensor_tick', '0.033')  # 30Hz
+        camera_bp.set_attribute('fov', '100')
+        camera_bp.set_attribute('sensor_tick', '0.033')
         self.camera = self.world.spawn_actor(
             camera_bp, carla.Transform(carla.Location(x=2.0, z=1.5)), attach_to=self.vehicle
         )
         self.camera.listen(self.process_image)
 
-        # 激光雷达（0.9.11兼容参数：用upper_fov和lower_fov替代vertical_fov）
+        # 激光雷达（0.9.11兼容参数）
         lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast')
         lidar_bp.set_attribute('channels', '32')
         lidar_bp.set_attribute('range', '50')
         lidar_bp.set_attribute('points_per_second', '100000')
         lidar_bp.set_attribute('rotation_frequency', '10')
         lidar_bp.set_attribute('horizontal_fov', '360')
-        # 0.9.11专用：垂直视野通过上下视角差定义（15 - (-15) = 30度）
-        lidar_bp.set_attribute('upper_fov', '15.0')   # 上视角
-        lidar_bp.set_attribute('lower_fov', '-15.0')  # 下视角
+        lidar_bp.set_attribute('upper_fov', '15.0')
+        lidar_bp.set_attribute('lower_fov', '-15.0')
         self.lidar = self.world.spawn_actor(
             lidar_bp, carla.Transform(carla.Location(x=0.0, z=2.0)), attach_to=self.vehicle
         )
@@ -221,20 +269,24 @@ def get_obstacle_directions(self, lidar_distances):
 
     def step(self, action=None):
         """执行动作（使用自动驾驶时可忽略action）"""
-        # 当启用autopilot时，不需要手动控制
         observation = self.get_observation()
         reward = 1.0
         done = False
         return observation, reward, done, {}
 
     def close(self):
-        """清理资源并恢复世界设置"""
+        """清理所有资源（包括NPC）"""
         # 销毁传感器
         for sensor in [self.camera, self.lidar, self.imu]:
             if sensor is not None and sensor.is_alive:
                 sensor.stop()
                 sensor.destroy()
-        # 销毁车辆
+        # 销毁NPC车辆
+        for npc in self.npc_vehicles:
+            if npc and npc.is_alive:
+                npc.destroy()
+        self.npc_vehicles.clear()
+        # 销毁主角车辆
         if self.vehicle is not None and self.vehicle.is_alive:
             self.vehicle.destroy()
         # 清空队列
@@ -245,4 +297,4 @@ def close(self):
         if self.settings:
             self.settings.synchronous_mode = False
             self.world.apply_settings(self.settings)
-        print("[资源清理] 所有传感器和车辆已销毁")
\ No newline at end of file
+        print("[资源清理] 所有传感器、车辆和NPC已销毁")
\ No newline at end of file

From 30e96eb261f798314d4767e4d683bdd96e10fe48 Mon Sep 17 00:00:00 2001
From: Pan-j-l <43541417288@qq.com>
Date: Sun, 14 Dec 2025 23:13:57 +0800
Subject: [PATCH 19/19] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=8E=AF=E5=A2=83?=
 =?UTF-8?q?=E9=9A=BE=E5=BA=A6=EF=BC=88NPC=20=E6=95=B0=E9=87=8F=E5=A2=9E?=
 =?UTF-8?q?=E8=87=B3=2060=20=E8=BE=86=EF=BC=89+=20=E6=8F=90=E9=AB=98?=
 =?UTF-8?q?=E4=B8=BB=E8=BD=A6=E8=BE=86=E8=A1=8C=E9=A9=B6=E9=80=9F=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../carla_environment.py                      | 467 +++++++++++++-----
 1 file changed, 340 insertions(+), 127 deletions(-)

diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py
index 34d1a320b7..c8d46fd14c 100644
--- a/src/self_driving_car_navigation/carla_environment.py
+++ b/src/self_driving_car_navigation/carla_environment.py
@@ -10,73 +10,220 @@
 class CarlaEnvironment(gym.Env):
     def __init__(self):
         super(CarlaEnvironment, self).__init__()
+        # 基础属性
         self.client = None
         self.world = None
         self.blueprint_library = None
-        self.settings = None  # 用于保存世界设置
-        self._connect_carla()
-
-        # 观测空间定义
-        self.observation_space = gym.spaces.Dict({
-            'image': gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8),
-            'lidar_distances': gym.spaces.Box(low=0, high=50, shape=(360,), dtype=np.float32),
-            'imu': gym.spaces.Box(low=-10, high=10, shape=(6,), dtype=np.float32)
-        })
-
-        # 传感器、车辆和NPC实例
+        self.settings = None
         self.vehicle = None
+        self.npc_vehicles = []
         self.camera = None
         self.lidar = None
         self.imu = None
-        self.npc_vehicles = []  # 存储NPC车辆实例
+        self.spawn_points = []
+        # TM核心配置（0.9.11专用）
+        self.traffic_manager = None
+        self.tm_port = 8000
+        self.tm_seed = 0  # 固定TM种子，保证行为一致
         # 数据队列
         self.image_queue = Queue(maxsize=1)
         self.lidar_queue = Queue(maxsize=1)
         self.imu_queue = Queue(maxsize=1)
-        # 生成点
-        self.spawn_points = self.world.get_map().get_spawn_points()
-        print(f"[CARLA场景] 检测到 {len(self.spawn_points)} 个车辆生成点")
+        # 车辆控制相关
+        self.vehicle_control = carla.VehicleControl()
+        self.last_steer = 0.0
+
+        # 连接CARLA
+        self._connect_carla()
+        # 初始化TM（适配0.9.11 API）
+        self._init_traffic_manager()
+        # 定义观测空间
+        self.observation_space = gym.spaces.Dict({
+            'image': gym.spaces.Box(low=0, high=255, shape=(128, 128, 3), dtype=np.uint8),
+            'lidar_distances': gym.spaces.Box(low=0, high=50, shape=(360,), dtype=np.float32),
+            'imu': gym.spaces.Box(low=-10, high=10, shape=(6,), dtype=np.float32)
+        })
+        # 获取有效生成点（仅保留道路上的点，修改：补充到60个以适配60辆NPC）
+        self.spawn_points = self._get_valid_road_spawn_points()
+        print(f"[场景初始化] 有效道路生成点数量: {len(self.spawn_points)}")
         sys.stdout.flush()
 
     def _connect_carla(self):
-        """连接CARLA服务器，支持重试并启用同步模式"""
+        """连接CARLA（增加版本检查）"""
         retry_count = 3
         for i in range(retry_count):
             try:
                 print(f"[CARLA连接] 尝试第{i+1}次连接（localhost:2000）...")
                 self.client = carla.Client('localhost', 2000)
-                self.client.set_timeout(15.0)
+                self.client.set_timeout(20.0)
                 self.world = self.client.get_world()
                 self.blueprint_library = self.world.get_blueprint_library()
                 
-                # 清除地图中所有默认静态车辆
-                actors = self.world.get_actors()
-                for actor in actors:
-                    if actor.type_id.startswith('vehicle.'):  # 筛选所有车辆类型
-                        actor.destroy()
-                        print(f"[清除默认车辆] 销毁静态车辆（ID: {actor.id}）")
-                
-                # 启用同步模式（关键优化：解决数据不同步问题）
+                # 同步模式配置（0.9.11最优参数）
                 self.settings = self.world.get_settings()
-                self.settings.synchronous_mode = True
-                self.settings.fixed_delta_seconds = 1/30  # 固定30帧
+                self.settings.synchronous_mode = True  # 启用同步模式
+                self.settings.fixed_delta_seconds = 1/20  # 降低帧率，提升稳定性
+                self.settings.no_rendering_mode = False  # 必须开启渲染，否则TM可能失效
                 self.world.apply_settings(self.settings)
                 
-                print("[CARLA连接] 成功连接到模拟器并启用同步模式")
+                # 双重清理
+                self._clear_all_non_ego_actors()
+                time.sleep(0.5)
+                self.world.tick()  # 显式推进仿真帧
+                self._clear_all_non_ego_actors()
+                
+                # 版本检查
+                server_version = self.client.get_server_version()
+                print(f"[CARLA连接] 成功连接，服务器版本：{server_version}")
+                if "0.9.11" not in server_version:
+                    print("[警告] 检测到非0.9.11版本，可能存在兼容性问题！")
                 return
             except Exception as e:
                 print(f"[CARLA连接失败] {str(e)}")
                 if i == retry_count - 1:
-                    raise RuntimeError("无法连接CARLA，请检查模拟器是否启动")
+                    raise RuntimeError("无法连接CARLA，请检查模拟器是否启动（需0.9.11版本）")
                 time.sleep(2)
 
+    def _init_traffic_manager(self):
+        """初始化交通管理器（严格适配0.9.11 API）"""
+        self.traffic_manager = self.client.get_trafficmanager(self.tm_port)
+        # 全局TM参数（0.9.11支持的全局方法）
+        self.traffic_manager.set_global_distance_to_leading_vehicle(2.0)  # 跟车距离（float）
+        self.traffic_manager.set_synchronous_mode(True)  # 同步模式（bool）
+        self.traffic_manager.set_random_device_seed(self.tm_seed)  # 随机种子（int）
+        self.traffic_manager.global_percentage_speed_difference(0.0)  # 全速行驶（float）
+        # 混合物理模式（0.9.11支持）
+        self.traffic_manager.set_hybrid_physics_mode(True)
+        self.traffic_manager.set_hybrid_physics_radius(50.0)
+        print("[TM配置] 交通管理器初始化完成（适配0.9.11 API）")
+
+    def _set_actor_tm_params(self, actor):
+        """为单个Actor设置TM参数（核心修改：提升主车辆速度，遵守交通规则）"""
+        if not self._is_actor_alive(actor):
+            return
+        try:
+            # 关键修改：设置为0%忽略交通规则，使车辆遵守红绿灯和标志
+            self.traffic_manager.ignore_lights_percentage(actor, 0.0)  # 不忽略交通灯
+            self.traffic_manager.ignore_signs_percentage(actor, 0.0)   # 不忽略交通标志
+            self.traffic_manager.ignore_walkers_percentage(actor, 0.0) # 不忽略行人
+            # 允许变道（float百分比）
+            self.traffic_manager.allow_vehicle_lane_change(actor, 100.0)
+            
+            # 核心修改：提升速度参数（区分主车辆和NPC）
+            if actor.attributes.get('role_name') == 'ego_vehicle':
+                # 主车辆：速度限制因子提升到1.8（超速80%），最高速度设为100km/h
+                self.traffic_manager.set_speed_limit_factor(actor, 1.8)
+                self.traffic_manager.set_speed_limit(actor, 100.0)
+            else:
+                # NPC车辆：保持原有参数（可选：也可适当提升）
+                self.traffic_manager.set_speed_limit_factor(actor, 1.2)
+                self.traffic_manager.set_speed_limit(actor, 60.0)
+                
+        except Exception as e:
+            print(f"[TM Actor配置警告] Actor ID {actor.id}: {str(e)}")
+
+    def _update_vehicle_steering(self, vehicle):
+        """更新车辆转向角，使轮胎转动跟随车辆运动"""
+        if not self._is_actor_alive(vehicle):
+            return
+            
+        try:
+            # 获取车辆当前速度和变换
+            velocity = vehicle.get_velocity()
+            speed = np.linalg.norm([velocity.x, velocity.y, velocity.z])
+            
+            # 获取车辆的物理控制
+            physics_control = vehicle.get_physics_control()
+            
+            # 根据自动驾驶的控制指令获取转向角
+            control = vehicle.get_control()
+            
+            # 平滑转向角变化，避免突变
+            steer_factor = 0.3  # 转向灵敏度
+            self.last_steer = self.last_steer * (1 - steer_factor) + control.steer * steer_factor
+            
+            # 应用转向角到所有车轮
+            for wheel in physics_control.wheels:
+                if wheel.type == carla.WheelType.Front:  # 只控制前轮转向
+                    wheel.steer_angle = self.last_steer * 70  # 70度最大转向角
+            
+            # 应用物理控制
+            vehicle.apply_physics_control(physics_control)
+            
+        except Exception as e:
+            print(f"[车辆转向更新错误] {str(e)}")
+
+    def _get_valid_road_spawn_points(self):
+        """过滤生成点：仅保留道路网络上的有效点（修改：补充到60个以适配60辆NPC）"""
+        map = self.world.get_map()
+        valid_points = []
+        for sp in map.get_spawn_points():
+            # 获取生成点对应的道路点
+            waypoint = map.get_waypoint(sp.location)
+            if waypoint and waypoint.road_id != -1:  # 确保在道路上
+                valid_points.append(sp)
+        # 若有效点不足60，补充随机道路点（修改：从30改为60）
+        if len(valid_points) < 60:
+            for _ in range(60 - len(valid_points)):
+                random_loc = self.world.get_random_location_from_navigation()
+                if random_loc:
+                    waypoint = map.get_waypoint(random_loc)
+                    valid_points.append(carla.Transform(waypoint.transform.location, waypoint.transform.rotation))
+        return valid_points
+
+    def _is_actor_alive(self, actor):
+        """安全检查Actor是否存活"""
+        try:
+            return actor is not None and actor.is_alive
+        except Exception:
+            return False
+
+    def _safe_destroy_actor(self, actor):
+        """安全销毁Actor"""
+        try:
+            if self._is_actor_alive(actor):
+                actor.destroy()
+        except Exception as e:
+            if "has been destroyed" not in str(e) and "not found" not in str(e):
+                print(f"[安全销毁警告] {str(e)}")
+
+    def _clear_all_non_ego_actors(self):
+        """清理非主车辆和NPC的Actor"""
+        if not self.world:
+            return
+        actors = self.world.get_actors()
+        cleared_count = {'vehicle': 0, 'bicycle': 0, 'static_vehicle': 0}
+        keep_ids = set()
+        if self._is_actor_alive(self.vehicle):
+            keep_ids.add(self.vehicle.id)
+        for npc in self.npc_vehicles:
+            if self._is_actor_alive(npc):
+                keep_ids.add(npc.id)
+        
+        for actor in actors:
+            try:
+                actor_type = actor.type_id
+                if actor_type.startswith('vehicle.') and actor.id not in keep_ids:
+                    self._safe_destroy_actor(actor)
+                    cleared_count['vehicle'] += 1
+                elif actor_type.startswith('walker.bicycle') and actor.id not in keep_ids:
+                    self._safe_destroy_actor(actor)
+                    cleared_count['bicycle'] += 1
+                elif actor_type.startswith('static.vehicle') and actor.id not in keep_ids:
+                    self._safe_destroy_actor(actor)
+                    cleared_count['static_vehicle'] += 1
+            except Exception as e:
+                if "has been destroyed" not in str(e) and "not found" not in str(e):
+                    print(f"[销毁Actor警告] {str(e)}")
+        
+        self.world.tick()  # 显式推进仿真帧
+        print(f"[清理] 车辆{cleared_count['vehicle']} | 自行车{cleared_count['bicycle']} | 静态车辆{cleared_count['static_vehicle']}")
+
     def process_image(self, image):
-        """处理摄像头数据，转换为RGB格式"""
+        """处理摄像头数据"""
         try:
-            array = np.frombuffer(image.raw_data, dtype=np.uint8)
-            array = np.reshape(array, (image.height, image.width, 4))
-            array = array[:, :, :3]
-            array = array[:, :, ::-1].copy()  # BGR转RGB
+            array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))[:, :, :3]
+            array = array[:, :, ::-1].copy()
             if self.image_queue.full():
                 self.image_queue.get()
             self.image_queue.put(array)
@@ -84,7 +231,7 @@ def process_image(self, image):
             print(f"[图像处理错误] {str(e)}")
 
     def process_lidar(self, data):
-        """处理激光雷达数据，生成360度距离数组"""
+        """处理激光雷达数据"""
         try:
             points = np.frombuffer(data.raw_data, dtype=np.dtype('f4')).reshape(-1, 4)[:, :3]
             distances = np.linalg.norm(points, axis=1)
@@ -104,7 +251,7 @@ def process_lidar(self, data):
             print(f"[激光雷达处理错误] {str(e)}")
 
     def process_imu(self, data):
-        """处理IMU数据，提取加速度和角速度"""
+        """处理IMU数据"""
         try:
             imu_data = np.array([
                 data.accelerometer.x, data.accelerometer.y, data.accelerometer.z,
@@ -117,134 +264,144 @@ def process_imu(self, data):
             print(f"[IMU处理错误] {str(e)}")
 
     def reset(self):
-        """重置环境，生成车辆、传感器和NPC（启用自动驾驶）"""
+        """重置环境（修改：生成主车辆+60辆NPC）"""
         self.close()
-        time.sleep(0.5)
+        time.sleep(1.0)
+        self._clear_all_non_ego_actors()
         self._spawn_vehicle()
+        
         if self.vehicle:
             self._spawn_sensors()
-            # 启用主角车自动驾驶
-            self.vehicle.set_autopilot(True)
-            # 生成NPC车辆（20辆）
-            self._spawn_npcs(20)
-        # 额外同步确保所有车辆启动
-        for _ in range(2):
-            self.world.tick()
+            # 启用物理模拟
+            self.vehicle.set_simulate_physics(True)
+            # 主车辆自动驾驶（延迟绑定+TM参数配置）
             time.sleep(0.5)
+            self.vehicle.set_autopilot(True, self.tm_port)  # 启用自动控制指令输入
+            self._set_actor_tm_params(self.vehicle)  # 为主车辆配置TM参数
+            # 生成NPC车辆（核心修改：从20辆改为60辆）
+            self._spawn_npcs(60)
+            self._clear_all_non_ego_actors()
+        
+        # 多次同步，确保物理生效
+        for _ in range(5):
+            self.world.tick()  # 显式推进仿真帧
+            time.sleep(0.2)
+        
+        print(f"[环境重置] 完成，主车辆1辆，NPC车辆{len(self.npc_vehicles)}辆")
         return self.get_observation()
 
     def _spawn_vehicle(self):
-        """生成主角车辆（特斯拉Model3）- 选择车道内的生成点"""
+        """生成主车辆（特斯拉Model3）"""
+        self._safe_destroy_actor(self.vehicle)
+        self.world.tick()  # 显式推进仿真帧
+
         vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
         vehicle_bp.set_attribute('color', '255,0,0')
         vehicle_bp.set_attribute('role_name', 'ego_vehicle')
 
-        # 优先选择车道内的生成点（减少初始偏离）
-        if self.spawn_points:
-            random.shuffle(self.spawn_points)
-            # 尝试前10个生成点，确保车辆在道路上
-            for spawn_point in self.spawn_points[:10]:
-                self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
-                if self.vehicle:
-                    self.vehicle.set_autopilot(False)
-                    self.vehicle.set_simulate_physics(True)
-                    print(f"[车辆生成] 成功在道路生成点生成（ID: {self.vehicle.id}）")
-                    return
-        
-        # 备用生成逻辑
-        spawn_index = 10
-        for i in range(3):
-            spawn_point = self.spawn_points[(spawn_index + i) % len(self.spawn_points)]
+        if not self.spawn_points:
+            raise RuntimeError("无可用道路生成点")
+
+        random.shuffle(self.spawn_points)
+        for spawn_point in self.spawn_points[:10]:
             self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point)
             if self.vehicle:
-                self.vehicle.set_autopilot(False)
-                self.vehicle.set_simulate_physics(True)
-                print(f"[车辆生成] 使用备用位置（ID: {self.vehicle.id}）")
+                self.vehicle.set_simulate_physics(True)  # 启用物理模拟
+                print(f"[主车辆生成] 成功（ID: {self.vehicle.id}）")
                 return
-        
-        raise RuntimeError("车辆生成失败，请重启CARLA或更换场景")
+
+        raise RuntimeError("主车辆生成失败，请重启CARLA")
 
     def _spawn_npcs(self, count):
-        """生成指定数量的NPC车辆并激活自动驾驶"""
+        """生成NPC车辆（0.9.11关键：生成后延迟启用自动驾驶+单个Actor配置TM）"""
         if not self.spawn_points:
-            print("[NPC生成] 没有可用的生成点")
+            print("[NPC生成] 无可用生成点")
             return
 
-        # 筛选可用车辆蓝图（排除主角车）
-        vehicle_blueprints = self.blueprint_library.filter('vehicle')
-        vehicle_blueprints = [bp for bp in vehicle_blueprints if bp.id != 'vehicle.tesla.model3']
-        if not vehicle_blueprints:
-            print("[NPC生成] 没有可用的NPC蓝图")
-            return
+        # 过滤主车辆附近的点（修改：将距离从20米改为15米，释放更多生成点）
+        ego_transform = self.vehicle.get_transform()
+        available_spawn_points = []
+        for sp in self.spawn_points:
+            distance = np.linalg.norm([
+                sp.location.x - ego_transform.location.x,
+                sp.location.y - ego_transform.location.y
+            ])
+            if distance > 15.0:  # 从20米→15米，增加可用生成点数量
+                available_spawn_points.append(sp)
+
+        if len(available_spawn_points) < count:
+            count = len(available_spawn_points)
+            print(f"[NPC生成] 可用点不足，生成{count}辆")
 
-        print(f"[NPC生成] 开始生成{count}辆NPC车辆...")
+        # 随机车辆蓝图
+        vehicle_bps = [bp for bp in self.blueprint_library.filter('vehicle.*') 
+                       if bp.has_attribute('color') and bp.id != 'vehicle.tesla.model3']
+        random.shuffle(vehicle_bps)
+        if not vehicle_bps:
+            vehicle_bps = self.blueprint_library.filter('vehicle.*')
+
+        # 生成NPC
         spawned_count = 0
-        used_spawn_points = []  # 避免生成点重叠
+        for i, spawn_point in enumerate(random.sample(available_spawn_points, count)):
+            bp = vehicle_bps[i % len(vehicle_bps)]
+            if bp.has_attribute('color'):
+                color = random.choice(bp.get_attribute('color').recommended_values)
+                bp.set_attribute('color', color)
+            bp.set_attribute('role_name', 'npc_vehicle')
 
-        # 循环尝试生成
-        while spawned_count < count and len(used_spawn_points) < len(self.spawn_points):
-            # 随机选择未使用的生成点
-            spawn_point = random.choice([p for p in self.spawn_points if p not in used_spawn_points])
-            used_spawn_points.append(spawn_point)
-            
-            # 随机选择车辆蓝图
-            npc_bp = random.choice(vehicle_blueprints)
-            
-            # 尝试生成NPC
-            npc = self.world.try_spawn_actor(npc_bp, spawn_point)
-            if npc:
-                self.npc_vehicles.append(npc)
-                # 启用自动驾驶
-                npc.set_autopilot(True)
+            npc_vehicle = self.world.try_spawn_actor(bp, spawn_point)
+            if npc_vehicle:
+                npc_vehicle.set_simulate_physics(True)  # 启用物理模拟
+                # 0.9.11核心：生成后延迟0.1秒启用自动驾驶，让物理模拟生效
+                time.sleep(0.1)
+                npc_vehicle.set_autopilot(True, self.tm_port)  # 启用自动控制指令输入
+                # 为单个NPC配置TM参数（关键：解决不动问题）
+                self._set_actor_tm_params(npc_vehicle)
+                self.npc_vehicles.append(npc_vehicle)
                 spawned_count += 1
-                
-                # 每生成5辆同步一次
+
                 if spawned_count % 5 == 0:
-                    self.world.tick()
-                    time.sleep(0.1)
+                    self.world.tick()  # 显式推进仿真帧
 
-        # 生成完成后同步
-        self.world.tick()
-        print(f"[NPC生成] 完成，实际生成{spawned_count}辆NPC")
+        self.world.tick()  # 显式推进仿真帧
+        print(f"[NPC生成] 成功生成{spawned_count}辆（目标：{count}辆）")
 
     def _spawn_sensors(self):
-        """生成传感器（适配0.9.11版本参数）"""
-        # 前视摄像头
+        """生成传感器"""
+        for sensor in [self.camera, self.lidar, self.imu]:
+            self._safe_destroy_actor(sensor)
+
+        # 摄像头
         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', '100')
-        camera_bp.set_attribute('sensor_tick', '0.033')
         self.camera = self.world.spawn_actor(
             camera_bp, carla.Transform(carla.Location(x=2.0, z=1.5)), attach_to=self.vehicle
         )
         self.camera.listen(self.process_image)
 
-        # 激光雷达（0.9.11兼容参数）
+        # 激光雷达
         lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast')
         lidar_bp.set_attribute('channels', '32')
         lidar_bp.set_attribute('range', '50')
         lidar_bp.set_attribute('points_per_second', '100000')
         lidar_bp.set_attribute('rotation_frequency', '10')
-        lidar_bp.set_attribute('horizontal_fov', '360')
-        lidar_bp.set_attribute('upper_fov', '15.0')
-        lidar_bp.set_attribute('lower_fov', '-15.0')
         self.lidar = self.world.spawn_actor(
             lidar_bp, carla.Transform(carla.Location(x=0.0, z=2.0)), attach_to=self.vehicle
         )
         self.lidar.listen(self.process_lidar)
 
-        # IMU传感器
+        # IMU
         imu_bp = self.blueprint_library.find('sensor.other.imu')
-        imu_bp.set_attribute('sensor_tick', '0.033')
         self.imu = self.world.spawn_actor(
             imu_bp, carla.Transform(), attach_to=self.vehicle
         )
         self.imu.listen(self.process_imu)
-        print("[传感器] 初始化成功")
+        print("[传感器] 初始化完成")
 
     def get_observation(self):
-        """获取传感器数据（确保数据就绪）"""
+        """获取观测数据"""
         while self.image_queue.empty() or self.lidar_queue.empty() or self.imu_queue.empty():
             time.sleep(0.01)
         return {
@@ -254,12 +411,11 @@ def get_observation(self):
         }
 
     def get_obstacle_directions(self, lidar_distances):
-        """计算四个方向的最近障碍物距离"""
+        """计算四向障碍物距离"""
         front_angles = np.concatenate([np.arange(345, 360), np.arange(0, 16)])
         rear_angles = np.arange(165, 196)
         left_angles = np.arange(75, 106)
         right_angles = np.arange(255, 286)
-
         return {
             'front': np.min(lidar_distances[front_angles]),
             'rear': np.min(lidar_distances[rear_angles]),
@@ -268,33 +424,90 @@ def get_obstacle_directions(self, lidar_distances):
         }
 
     def step(self, action=None):
-        """执行动作（使用自动驾驶时可忽略action）"""
+        """环境交互步骤（0.9.11同步关键）"""
+        # 同步世界（TM会自动同步）
+        self.world.tick()  # 显式推进仿真帧
+        
+        # 更新主车辆轮胎转向
+        if self._is_actor_alive(self.vehicle):
+            self._update_vehicle_steering(self.vehicle)
+            
+        # 更新NPC车辆轮胎转向
+        for npc in self.npc_vehicles:
+            if self._is_actor_alive(npc):
+                self._update_vehicle_steering(npc)
+        
+        # 清理无效NPC
+        self.npc_vehicles = [npc for npc in self.npc_vehicles if self._is_actor_alive(npc)]
+        
+        # 打印NPC速度（调试用）
+        if random.random() < 0.1:  # 10%概率打印
+            for npc in self.npc_vehicles[:1]:
+                try:
+                    velocity = npc.get_velocity()
+                    speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) * 3.6  # m/s → km/h
+                    print(f"[NPC速度] ID:{npc.id} 速度:{speed:.1f}km/h")
+                except Exception:
+                    pass
+        
+        # 获取观测
         observation = self.get_observation()
         reward = 1.0
         done = False
         return observation, reward, done, {}
 
     def close(self):
-        """清理所有资源（包括NPC）"""
+        """清理资源"""
+        # 停止TM
+        if self.traffic_manager:
+            try:
+                self.traffic_manager.set_synchronous_mode(False)
+            except Exception as e:
+                print(f"[TM清理警告] {str(e)}")
         # 销毁传感器
         for sensor in [self.camera, self.lidar, self.imu]:
-            if sensor is not None and sensor.is_alive:
-                sensor.stop()
-                sensor.destroy()
-        # 销毁NPC车辆
+            self._safe_destroy_actor(sensor)
+        # 销毁NPC
         for npc in self.npc_vehicles:
-            if npc and npc.is_alive:
-                npc.destroy()
-        self.npc_vehicles.clear()
-        # 销毁主角车辆
-        if self.vehicle is not None and self.vehicle.is_alive:
-            self.vehicle.destroy()
+            self._safe_destroy_actor(npc)
+        self.npc_vehicles = []
+        # 销毁主车辆
+        self._safe_destroy_actor(self.vehicle)
+        self.vehicle = None
+        # 最后清理
+        self._clear_all_non_ego_actors()
         # 清空队列
         for q in [self.image_queue, self.lidar_queue, self.imu_queue]:
             while not q.empty():
                 q.get()
         # 恢复世界设置
         if self.settings:
-            self.settings.synchronous_mode = False
-            self.world.apply_settings(self.settings)
-        print("[资源清理] 所有传感器、车辆和NPC已销毁")
\ No newline at end of file
+            try:
+                self.settings.synchronous_mode = False
+                self.world.apply_settings(self.settings)
+            except Exception as e:
+                print(f"[世界设置恢复警告] {str(e)}")
+        print("[资源清理] 所有资源已销毁")
+
+
+if __name__ == "__main__":
+    # 测试环境
+    try:
+        env = CarlaEnvironment()
+        print("环境初始化完成，开始测试...")
+        obs = env.reset()
+        print(f"观测数据：图像{obs['image'].shape}，激光雷达{obs['lidar_distances'].shape}，IMU{obs['imu'].shape}")
+        # 运行600步（约30秒）
+        for i in range(600):
+            obs, reward, done, _ = env.step()
+            if i % 50 == 0:
+                obstacle_info = env.get_obstacle_directions(obs['lidar_distances'])
+                print(f"第{i}步 - 前向距离：{obstacle_info['front']:.2f}m，NPC数量：{len(env.npc_vehicles)}")
+            time.sleep(0.05)
+        env.close()
+        print("测试完成")
+    except Exception as e:
+        print(f"测试出错：{str(e)}")
+        if 'env' in locals():
+            env.close()
+        sys.exit(1)
\ No newline at end of file