From a1a6ffec0e2f211e704b7706c7901763c8ab6f4b Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Thu, 18 Dec 2025 21:34:07 +0800 Subject: [PATCH 1/7] =?UTF-8?q?=E8=A7=84=E8=8C=83=20ROS=20=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=E5=8C=85=E7=9A=84=E6=A0=B8=E5=BF=83=E9=85=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros/CMakeLists.txt | 37 +++++++++++++++++++ .../ros/package.xml | 24 ++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 src/self_driving_car_navigation/ros/CMakeLists.txt create mode 100644 src/self_driving_car_navigation/ros/package.xml diff --git a/src/self_driving_car_navigation/ros/CMakeLists.txt b/src/self_driving_car_navigation/ros/CMakeLists.txt new file mode 100644 index 0000000000..51673553ab --- /dev/null +++ b/src/self_driving_car_navigation/ros/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.0.2) +# 功能包名称（必须与package.xml中的name一致） +project(mmap_test) + +# -------------------------- 适配Python3.7.5 -------------------------- +# 指定你的虚拟环境Python解释器路径（关键！） +set(PYTHON_EXECUTABLE /home/pan-j-l/my_ros_project/venv_py37/bin/python3) +# 系统Python3.7的头文件路径（Ubuntu20.04默认路径，一般无需修改） +set(PYTHON_INCLUDE_DIR /usr/include/python3.7m) +# 系统Python3.7的库文件路径（Ubuntu20.04默认路径，一般无需修改） +set(PYTHON_LIBRARY /usr/lib/x86_64-linux-gnu/libpython3.7m.so) + +# -------------------------- 查找ROS依赖包 -------------------------- +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + cv_bridge +) + +# -------------------------- 声明Catkin包 -------------------------- +catkin_package( + # 声明该包依赖的其他ROS包，供其他包引用 + CATKIN_DEPENDS rospy std_msgs sensor_msgs cv_bridge +) + +# -------------------------- 安装Python脚本 -------------------------- +# 将scripts目录下的Python脚本安装到ROS的devel目录，实现全局调用 +catkin_install_python(PROGRAMS + scripts/ros_test_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# -------------------------- 包含目录 -------------------------- +include_directories( + ${catkin_INCLUDE_DIRS} +) \ No newline at end of file diff --git a/src/self_driving_car_navigation/ros/package.xml b/src/self_driving_car_navigation/ros/package.xml new file mode 100644 index 0000000000..2be4bc3fea --- /dev/null +++ b/src/self_driving_car_navigation/ros/package.xml @@ -0,0 +1,24 @@ + + + + mmap_test + 0.0.1 + ROS Noetic功能包：集成感知模块与决策模块的测试节点 + + pan-j-l + MIT + + + catkin + + + rospy + std_msgs + sensor_msgs + cv_bridge + + + + catkin + + \ No newline at end of file From ce343b90116217cd0daa5420b90424a7a39baa81 Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Fri, 19 Dec 2025 10:56:33 +0800 Subject: [PATCH 2/7] =?UTF-8?q?=E9=87=8D=E6=9E=84ROS=E8=8A=82=E7=82=B9?= =?UTF-8?q?=E4=B8=BA=E7=B1=BB=E5=BC=8F=E5=B0=81=E8=A3=85=EF=BC=8C=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=E6=A8=A1=E6=8B=9F=E6=95=B0=E6=8D=AE=E7=89=88perceptio?= =?UTF-8?q?n=5Fdecision=5Fnode=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros/perception_decision_node.py | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 src/self_driving_car_navigation/ros/perception_decision_node.py diff --git a/src/self_driving_car_navigation/ros/perception_decision_node.py b/src/self_driving_car_navigation/ros/perception_decision_node.py new file mode 100644 index 0000000000..df5031e0b8 --- /dev/null +++ b/src/self_driving_car_navigation/ros/perception_decision_node.py @@ -0,0 +1,106 @@ +#!/home/pan-j-l/my_ros_project/venv_py37/bin/python3 +# 适配你的虚拟环境Python解释器路径（关键！避免环境依赖问题） +import rospy +import torch +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Image, Imu, LaserScan +# 导入自定义感知/决策模块（路径适配你的scripts/models目录） +from models.perception_module import PerceptionModule +from models.decision_module import DecisionModule + +class PerceptionDecisionNode: + def __init__(self): + # 节点初始化 + rospy.init_node('perception_decision_node', anonymous=True) + self.node_name = rospy.get_name() + rospy.loginfo(f"[{self.node_name}] 节点初始化成功！") + + # 加载参数（带默认值，可通过命令行/launch动态修改） + self.image_shape = rospy.get_param('~image_shape', (3, 128, 128)) + self.feature_dim = rospy.get_param('~feature_dim', 128) + self.cmd_dim = rospy.get_param('~cmd_dim', 2) + self.timer_freq = rospy.get_param('~timer_freq', 10.0) + + # 打印加载的参数（日志格式和你预期的一致） + rospy.loginfo(f"[{self.node_name}] 加载参数完成：") + rospy.loginfo(f" - 图像形状：{self.image_shape}") + rospy.loginfo(f" - 特征维度：{self.feature_dim}") + rospy.loginfo(f" - 指令维度：{self.cmd_dim}") + rospy.loginfo(f" - 定时器频率：{self.timer_freq}Hz") + + # 初始化感知/决策模块 + try: + self.perception = PerceptionModule( + image_input_shape=self.image_shape, + output_feature_dim=self.feature_dim + ) + self.decision = DecisionModule( + input_feature_dim=self.feature_dim, + output_cmd_dim=self.cmd_dim + ) + rospy.loginfo(f"[{self.node_name}] 感知/决策模块初始化成功！") + except Exception as e: + rospy.logfatal(f"[{self.node_name}] 模块初始化失败：{str(e)}") + exit(1) # 模块初始化失败直接退出 + + # 初始化控制指令发布者（发布到/cmd_vel话题） + self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) + rospy.loginfo(f"[{self.node_name}] 发布者初始化成功：/cmd_vel话题") + + # 初始化定时器（按设定频率执行数据处理逻辑） + self.timer = rospy.Timer( + rospy.Duration(1.0 / self.timer_freq), + self.timer_callback + ) + rospy.loginfo(f"[{self.node_name}] 定时器初始化成功：{self.timer_freq}Hz") + rospy.loginfo(f"[{self.node_name}] 节点开始运行（按Ctrl+C退出）...") + + def timer_callback(self, event): + """定时器回调：核心逻辑（模拟数据→感知→决策→发布指令）""" + # ========== 跳过传感器等待：直接生成模拟数据 ========== + # 模拟IMU数据（1个batch，6维：线加速度x/y/z + 角速度x/y/z） + self.imu_data = torch.randn(1, 6) + # 模拟图像数据（1个batch，对应image_shape参数） + self.image_data = torch.randn(1, *self.image_shape) + # 模拟激光雷达数据（1个batch，360个扫描点） + self.lidar_data = torch.randn(1, 360) + + try: + # 1. 感知模块：融合多传感器数据生成特征 + fused_feature = self.perception.process( + self.imu_data, + self.image_data, + self.lidar_data + ) + + # 2. 决策模块：根据融合特征生成控制指令 + control_cmd = self.decision.get_control_cmd(fused_feature) + + # 3. 封装并发布ROS控制指令（Twist消息） + twist_msg = Twist() + twist_msg.linear.x = control_cmd['linear_x'] # 线速度x + twist_msg.angular.z = control_cmd['angular_z'] # 角速度z + self.cmd_pub.publish(twist_msg) + + # 4. 打印日志（格式和你预期的完全一致） + rospy.loginfo( + f"[{self.node_name}] 生成控制指令：线速度x={control_cmd['linear_x']:.2f}m/s，" + f"角速度z={control_cmd['angular_z']:.2f}rad/s" + ) + + except Exception as e: + # 异常捕获：避免单个错误导致节点崩溃 + rospy.logerr(f"[{self.node_name}] 指令生成失败：{str(e)}") + +if __name__ == '__main__': + try: + # 创建节点实例并运行 + node = PerceptionDecisionNode() + rospy.spin() + except rospy.ROSInterruptException: + # 捕获Ctrl+C中断，友好退出 + rospy.loginfo(f"[{rospy.get_name()}] 节点被中断，正常退出！") + except Exception as e: + # 捕获其他致命错误 + rospy.logfatal(f"节点启动失败：{str(e)}") + exit(1) \ No newline at end of file From aaeaaf7374d3a25253f6a2e8ff81d0ba5327174f Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Fri, 19 Dec 2025 15:31:51 +0800 Subject: [PATCH 3/7] =?UTF-8?q?=E6=8F=90=E4=BA=A4.launch=E6=96=87=E4=BB=B6?= =?UTF-8?q?=EF=BC=8C=E5=AE=9E=E7=8E=B0ROS=E8=8A=82=E7=82=B9=E4=B8=80?= =?UTF-8?q?=E9=94=AE=E5=90=AF=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros/launch/perception_decision.launch | 23 +++++++++++++++++++ .../ros/perception_decision_node.py | 13 +++++++---- 2 files changed, 31 insertions(+), 5 deletions(-) create mode 100644 src/self_driving_car_navigation/ros/launch/perception_decision.launch diff --git a/src/self_driving_car_navigation/ros/launch/perception_decision.launch b/src/self_driving_car_navigation/ros/launch/perception_decision.launch new file mode 100644 index 0000000000..f176b67d96 --- /dev/null +++ b/src/self_driving_car_navigation/ros/launch/perception_decision.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/self_driving_car_navigation/ros/perception_decision_node.py b/src/self_driving_car_navigation/ros/perception_decision_node.py index df5031e0b8..be0990594d 100644 --- a/src/self_driving_car_navigation/ros/perception_decision_node.py +++ b/src/self_driving_car_navigation/ros/perception_decision_node.py @@ -15,11 +15,14 @@ def __init__(self): self.node_name = rospy.get_name() rospy.loginfo(f"[{self.node_name}] 节点初始化成功！") - # 加载参数（带默认值，可通过命令行/launch动态修改） - self.image_shape = rospy.get_param('~image_shape', (3, 128, 128)) - self.feature_dim = rospy.get_param('~feature_dim', 128) - self.cmd_dim = rospy.get_param('~cmd_dim', 2) - self.timer_freq = rospy.get_param('~timer_freq', 10.0) + + image_shape_str = rospy.get_param('~image_shape', "[3, 128, 128]") # 读字符串 + self.image_shape = tuple(map(int, image_shape_str.strip('[]').replace(' ', '').split(','))) # 核心转换 + + # 2. 处理数值型参数（字符串→int/float） + self.feature_dim = int(rospy.get_param('~feature_dim', 128)) # 转整数 + self.cmd_dim = int(rospy.get_param('~cmd_dim', 2)) # 转整数 + self.timer_freq = float(rospy.get_param('~timer_freq', 10.0)) # 转浮点数 # 打印加载的参数（日志格式和你预期的一致） rospy.loginfo(f"[{self.node_name}] 加载参数完成：") From 484104fea2dc31f4f8a11189f697c42caecb6f00 Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Sat, 20 Dec 2025 10:35:10 +0800 Subject: [PATCH 4/7] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=88=91=E7=9A=84README.?= =?UTF-8?q?md=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/self_driving_car_navigation/README.md | 188 +++++++++++----------- 1 file changed, 92 insertions(+), 96 deletions(-) diff --git a/src/self_driving_car_navigation/README.md b/src/self_driving_car_navigation/README.md index 8fb3467c36..36bca70ea0 100644 --- a/src/self_driving_car_navigation/README.md +++ b/src/self_driving_car_navigation/README.md @@ -1,7 +1,11 @@ -# 自动驾驶仿真学习环境（基于Carla与Python3.7） +# 自动驾驶仿真学习环境（基于Carla与Python3.7，集成ROS） + +本项目基于[MMAP-DRL-Nav](https://github.com/CV4RA/MMAP-DRL-Nav)的核心架构，实现了一套“多模态感知+深度强化学习（DRL）+ROS部署”的自动驾驶导航系统。项目分为**Win11环境下的机器学习核心模块**（模型训练、CARLA仿真交互）和**Ubuntu ROS环境下的节点封装模块**（实时感知决策），可完成从多传感器数据融合到车辆控制指令输出的全流程，适用于自动驾驶导航算法的学习、验证与二次开发。 + + +## 项目核心目标 +通过融合视觉图像、激光雷达（LiDAR）、IMU等多源异构传感器数据，利用注意力机制完成特征融合，结合深度强化学习（DQN）训练决策模型，最终通过ROS节点输出车辆线速度/角速度控制指令，在CARLA仿真环境中验证“避障、路径跟踪、目标导航”等核心导航能力。 -## 项目概述 -本项目构建了基于Carla 0.9.11仿真平台和Python 3.7的自动驾驶学习环境，适用于车辆控制、场景感知、路径规划等自动驾驶相关算法的学习与实践。通过VSCode开发环境实现代码编写、调试与运行的一体化流程，支持从基础车辆控制到复杂场景仿真的完整学习路径。 ## 环境准备 @@ -25,111 +29,103 @@ pip install numpy opencv-python matplotlib vscode-debugpy 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. 感知模块 +- `perception_module.py`：处理多传感器输入（IMU、相机图像、激光雷达）的神经网络模块，包括： + - 用于视觉场景理解的语义分割网络 + - 基于1D卷积的激光雷达特征提取 + - 处理激光雷达数据的障碍物检测子网络 + - 输出场景信息、分割结果、里程计、障碍物和边界特征 -1. 启动Carla服务器： -```bash -# 在Carla安装目录下执行 -./CarlaUE4.sh # Linux/Mac -CarlaUE4.exe # Windows -``` +- `ros/perception_module.py`：ROS优化的感知模块，具有： + - 传感器数据扁平化与拼接处理 + - 基于线性层的特征融合 + - 使用`cv_bridge`实现ROS图像与张量的转换 -2. 基础车辆控制示例： -```bash -python main.py --mode manual # 手动控制模式 -python main.py --mode auto # 自动控制模式 -``` -3. 场景仿真运行： -```bash -python scene_generation.py --weather rain --obstacles 5 -``` +### 2. 跨域注意力融合 +- `attention_module.py`：实现`CrossDomainAttention`类用于多模态特征融合： + - 将异质输入（4D/2D张量）调整为统一的3D格式`[batch, seq_len, features]` + - 通过动态线性层对齐特征维度 + - 使用带残差连接和层归一化的多头自注意力块 + - 拼接并融合场景、分割、里程计、障碍物和边界数据的特征 -4. 传感器数据采集： -```bash -python sensor_manager.py --record --output ./data -``` -### VSCode开发提示 -- 按`F5`启动调试模式（需配置`.vscode/launch.json`） -- 使用Code Runner插件（右键`Run Code`）快速执行单文件 -- 推荐使用VSCode的Jupyter插件进行分步调试与数据可视化 +### 3. 决策模块 +- `decision_module.py`：核心决策网络，具有： + - 生成动作分布（转向角、油门）的策略网络 + - 从融合特征估计状态价值的价值网络 + - 通过均值池化聚合时序特征 + +- `ros/decision_module.py`：兼容ROS的决策模块： + - 用于生成控制指令的序列线性层 + - 将输出限制在安全范围内（线速度：0~2m/s，角速度：-1~1rad/s） + - 返回字典形式的控制指令，便于转换为ROS消息 + + +### 4. 自评估梯度模型 +- `sagm.py`：实现`SelfAssessmentGradientModel`用于动作价值估计： + - 处理拼接的状态-动作特征的全连接网络 + - 输出Q值以评估动作质量 + - 与actor-critic框架集成，用于强化学习更新 + + +### 5. 系统集成 +- `main.py`：定义`IntegratedSystem`类，整合所有模块： + - 端到端流程：多模态输入→感知→注意力融合→决策→自评估 + - 支持MSE损失训练（策略回归+价值估计+Q值匹配） + - 包含带设备管理（CPU/GPU）的训练/测试循环 + + +## ROS集成 +- `ros/CMakeLists.txt`：配置ROS功能包编译： + - 指定Python 3.7环境及依赖（rospy、std_msgs、sensor_msgs、cv_bridge） + - 将Python脚本安装到ROS环境 + - 设置Catkin的包含目录 + +- `ros/package.xml`：ROS功能包元数据，包含依赖和维护者信息 + +- `ros/perception_decision_node.py`：主ROS节点： + - 订阅传感器话题（模拟或真实数据） + - 按指定频率（默认10Hz）运行感知/决策流程 + - 发布控制指令到`/cmd_vel`话题 + +- `ros/ros_test_node.py`：模块验证测试节点： + - 使用模拟传感器数据验证感知/决策工作流 + - 记录特征形状和控制指令日志 + + +## 仿真与训练 +- `carla_environment.py`：CARLA模拟器接口（兼容Gym）： + - 管理模拟器连接、车辆生成和传感器设置 + - 配置交通管理器（兼容CARLA 0.9.11）以控制NPC行为 + - 提供观测空间（图像、激光雷达、IMU）和动作接口 + +- `dataloader.py`：数据加载工具： + - `CarlaDataset`类用于处理模拟的CARLA数据 + - 生成图像、激光雷达、IMU和动作数据的批次 + +- `dqn_agent.py`：DQN强化学习实现： + - 经验回放缓冲区 + - ε-贪婪动作选择 + - 带伽马折扣因子的目标网络更新 + +- `run_simulation.py`：CARLA中平滑 spectator 相机控制工具，使用线性插值实现 -## 参数调整指南 -| 参数 | 调整范围 | 效果说明 | -|-------------------|----------|----------------------------------| -| `throttle_gain` | 0.1~1.0 | 增大会提高加速响应，过大会导致打滑 | -| `sensor_fps` | 10~60 | 提高值增加数据精度（增加计算量） | -| `obstacle_density`| 0~20 | 增大会增加场景复杂度 | -| `simulation_delta_seconds`| 0.01~0.1 | 减小值提高仿真精度（降低运行速度）| +## 核心工作流程 +1. **感知**：多传感器数据（图像/激光雷达/IMU）被处理为结构化特征 +2. **融合**：跨域注意力将异质特征合并为统一表示 +3. **决策**：策略网络从融合特征生成控制指令 +4. **评估**：SAGM评估动作质量以指导学习 +5. **ROS集成**：指令发布到车辆控制话题，实现实时执行 ## 参考资料 - [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 +- [项目来源](https://github.com/CV4RA/MMAP-DRL-Nav) \ No newline at end of file From ce801452e079cc5490e9e406fb67994e95262098 Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Wed, 24 Dec 2025 14:17:59 +0800 Subject: [PATCH 5/7] =?UTF-8?q?=E5=90=88=E5=B9=B6=20run=5Fsimulation=20?= =?UTF-8?q?=E4=B8=8E=20carla=5Fenvironment=20=E5=B9=B6=E8=A7=A3=E5=86=B3?= =?UTF-8?q?=E8=AD=A6=E5=91=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../{carla_environment.py => run_carla.py} | 418 +++++++++++------- .../run_simulation.py | 121 ----- 2 files changed, 263 insertions(+), 276 deletions(-) rename src/self_driving_car_navigation/{carla_environment.py => run_carla.py} (57%) delete 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/run_carla.py similarity index 57% rename from src/self_driving_car_navigation/carla_environment.py rename to src/self_driving_car_navigation/run_carla.py index 5d6cc466b4..480ac95f99 100644 --- a/src/self_driving_car_navigation/carla_environment.py +++ b/src/self_driving_car_navigation/run_carla.py @@ -4,8 +4,11 @@ import time import sys import random +import pygame +import os from queue import Queue from gym import spaces +from collections import deque class CarlaEnvironment(gym.Env): def __init__(self): @@ -21,7 +24,7 @@ def __init__(self): self.lidar = None self.imu = None self.spawn_points = [] - # TM核心配置（0.9.11专用） + # TM核心配置（0.9.11专用 - 仅保留全局配置） self.traffic_manager = None self.tm_port = 8000 self.tm_seed = 0 # 固定TM种子，保证行为一致 @@ -29,13 +32,10 @@ def __init__(self): self.image_queue = Queue(maxsize=1) self.lidar_queue = Queue(maxsize=1) self.imu_queue = Queue(maxsize=1) - # 车辆控制相关 - self.vehicle_control = carla.VehicleControl() - self.last_steer = 0.0 - # 连接CARLA + # 连接CARLA（强化严格同步模式） self._connect_carla() - # 初始化TM（适配0.9.11 API） + # 初始化TM（仅用0.9.11确实验证的全局API） self._init_traffic_manager() # 定义观测空间 self.observation_space = gym.spaces.Dict({ @@ -43,33 +43,38 @@ def __init__(self): '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） + # 获取有效生成点（仅保留道路上的点，补充到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(20.0) + self.client.set_timeout(30.0) self.world = self.client.get_world() self.blueprint_library = self.world.get_blueprint_library() - # 同步模式配置（0.9.11最优参数） + # 终极同步配置（消除所有帧差） self.settings = self.world.get_settings() - self.settings.synchronous_mode = True # 启用同步模式 - self.settings.fixed_delta_seconds = 1/20 # 降低帧率，提升稳定性 - self.settings.no_rendering_mode = False # 必须开启渲染，否则TM可能失效 + self.settings.synchronous_mode = True + self.settings.fixed_delta_seconds = 1/60 # 60FPS仿真，物理更平滑 + self.settings.no_rendering_mode = False + self.settings.substepping = True + self.settings.max_substep_delta_time = 0.003 # 更小的子步（3ms），过滤微米级抖动 + self.settings.max_substeps = 30 self.world.apply_settings(self.settings) + time.sleep(1.5) # 延长等待时间，确保配置完全生效 + current_settings = self.world.get_settings() + print(f"[同步模式] 生效状态: {current_settings.synchronous_mode}, 固定帧间隔: {current_settings.fixed_delta_seconds}s") # 双重清理 self._clear_all_non_ego_actors() - time.sleep(0.5) - self.world.tick() # 显式推进仿真帧 + self.world.tick() self._clear_all_non_ego_actors() # 版本检查 @@ -85,84 +90,31 @@ def _connect_carla(self): 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_global_distance_to_leading_vehicle(2.0) + self.traffic_manager.set_synchronous_mode(True) + self.traffic_manager.set_random_device_seed(self.tm_seed) + self.traffic_manager.global_percentage_speed_difference(0.0) self.traffic_manager.set_hybrid_physics_mode(True) self.traffic_manager.set_hybrid_physics_radius(50.0) - print("[TM配置] 交通管理器初始化完成（适配0.9.11 API）") + print("[TM配置] 交通管理器同步模式已启用") def _set_actor_tm_params(self, actor): - """为单个Actor设置TM参数（核心修改：提升主车辆速度，遵守交通规则）""" + """彻底移除所有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)}") + pass 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: # 确保在道路上 + if waypoint and waypoint.road_id != -1: valid_points.append(sp) - # 若有效点不足60，补充随机道路点（修改：从30改为60） + # 补充到60个 if len(valid_points) < 60: for _ in range(60 - len(valid_points)): random_loc = self.world.get_random_location_from_navigation() @@ -216,13 +168,12 @@ def _clear_all_non_ego_actors(self): if "has been destroyed" not in str(e) and "not found" not in str(e): print(f"[销毁Actor警告] {str(e)}") - self.world.tick() # 显式推进仿真帧 + self.world.tick() print(f"[清理] 车辆{cleared_count['vehicle']} | 自行车{cleared_count['bicycle']} | 静态车辆{cleared_count['static_vehicle']}") def process_image(self, image): - """处理摄像头数据 - 修复frame属性访问问题""" + """处理摄像头数据""" try: - # 检查是否为有效的图像对象 if not hasattr(image, 'raw_data') or not hasattr(image, 'height') or not hasattr(image, 'width'): print("[图像处理错误] 无效的图像数据") return @@ -236,9 +187,8 @@ def process_image(self, image): print(f"[图像处理错误] {str(e)}") def process_lidar(self, data): - """处理激光雷达数据 - 修复frame属性访问问题""" + """处理激光雷达数据""" try: - # 检查是否为有效的激光雷达数据 if not hasattr(data, 'raw_data'): print("[激光雷达处理错误] 无效的激光雷达数据") return @@ -261,9 +211,8 @@ def process_lidar(self, data): print(f"[激光雷达处理错误] {str(e)}") def process_imu(self, data): - """处理IMU数据 - 修复frame属性访问问题""" + """处理IMU数据""" try: - # 检查是否为有效的IMU数据 if not hasattr(data, 'accelerometer') or not hasattr(data, 'gyroscope'): print("[IMU处理错误] 无效的IMU数据") return @@ -279,7 +228,7 @@ def process_imu(self, data): print(f"[IMU处理错误] {str(e)}") def reset(self): - """重置环境（修改：生成主车辆+60辆NPC）""" + """重置环境""" self.close() time.sleep(1.0) self._clear_all_non_ego_actors() @@ -287,28 +236,44 @@ def reset(self): if self.vehicle: self._spawn_sensors() - # 启用物理模拟 self.vehicle.set_simulate_physics(True) - # 主车辆自动驾驶（延迟绑定+TM参数配置） + # 优化车辆物理参数（彻底消除微小抖动） + self._optimize_vehicle_physics() time.sleep(0.5) - self.vehicle.set_autopilot(True, self.tm_port) # 启用自动控制指令输入 - self._set_actor_tm_params(self.vehicle) # 为主车辆配置TM参数 - # 生成NPC车辆（核心修改：从20辆改为60辆） + self.vehicle.set_autopilot(True, self.tm_port) self._spawn_npcs(60) self._clear_all_non_ego_actors() - # 多次同步，确保物理生效 - for _ in range(5): - self.world.tick() # 显式推进仿真帧 - time.sleep(0.2) + # 多次同步，物理稳定（延长时间） + for _ in range(15): + self.world.tick() + time.sleep(0.05) print(f"[环境重置] 完成，主车辆1辆，NPC车辆{len(self.npc_vehicles)}辆") return self.get_observation() + def _optimize_vehicle_physics(self): + """终极物理优化：彻底消除车辆微小震动""" + if not self._is_actor_alive(self.vehicle): + return + try: + physics_control = self.vehicle.get_physics_control() + # 进一步降低悬挂刚度（从1000→800），减少微米级震动 + for wheel in physics_control.wheels: + wheel.suspension_stiffness = 800.0 # 更低的刚度，过滤路面微小颠簸 + wheel.suspension_damping = 250.0 # 更高的阻尼，快速衰减震动 + wheel.suspension_compression = 300.0 # 增加压缩阻尼 + wheel.max_suspension_travel = 0.08 # 减小悬挂行程，避免过度晃动 + wheel.friction_slip = 1.2 # 增加轮胎抓地力，减少打滑抖动 + self.vehicle.apply_physics_control(physics_control) + print("[物理优化] 车辆悬挂+轮胎参数已终极优化，消除微小震动") + except Exception as e: + print(f"[物理优化警告] {str(e)}") + def _spawn_vehicle(self): - """生成主车辆（特斯拉Model3）""" + """生成主车辆""" self._safe_destroy_actor(self.vehicle) - self.world.tick() # 显式推进仿真帧 + self.world.tick() vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3') vehicle_bp.set_attribute('color', '255,0,0') @@ -321,19 +286,18 @@ def _spawn_vehicle(self): 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_simulate_physics(True) # 启用物理模拟 + self.vehicle.set_simulate_physics(True) print(f"[主车辆生成] 成功（ID: {self.vehicle.id}）") return raise RuntimeError("主车辆生成失败，请重启CARLA") def _spawn_npcs(self, count): - """生成NPC车辆（0.9.11关键：生成后延迟启用自动驾驶+单个Actor配置TM）""" + """生成NPC车辆""" if not self.spawn_points: print("[NPC生成] 无可用生成点") return - # 过滤主车辆附近的点（修改：将距离从20米改为15米，释放更多生成点） ego_transform = self.vehicle.get_transform() available_spawn_points = [] for sp in self.spawn_points: @@ -341,21 +305,19 @@ def _spawn_npcs(self, count): sp.location.x - ego_transform.location.x, sp.location.y - ego_transform.location.y ]) - if distance > 15.0: # 从20米→15米，增加可用生成点数量 + if distance > 15.0: available_spawn_points.append(sp) if len(available_spawn_points) < count: count = len(available_spawn_points) print(f"[NPC生成] 可用点不足，生成{count}辆") - # 随机车辆蓝图 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 for i, spawn_point in enumerate(random.sample(available_spawn_points, count)): bp = vehicle_bps[i % len(vehicle_bps)] @@ -366,19 +328,16 @@ def _spawn_npcs(self, count): 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) + npc_vehicle.set_simulate_physics(True) + time.sleep(0.05) + npc_vehicle.set_autopilot(True, self.tm_port) self.npc_vehicles.append(npc_vehicle) spawned_count += 1 if spawned_count % 5 == 0: - self.world.tick() # 显式推进仿真帧 + self.world.tick() - self.world.tick() # 显式推进仿真帧 + self.world.tick() print(f"[NPC生成] 成功生成{spawned_count}辆（目标：{count}辆）") def _spawn_sensors(self): @@ -400,8 +359,8 @@ def _spawn_sensors(self): 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('points_per_second', '200000') + lidar_bp.set_attribute('rotation_frequency', '60') # 与仿真帧率一致 self.lidar = self.world.spawn_actor( lidar_bp, carla.Transform(carla.Location(x=0.0, z=2.0)), attach_to=self.vehicle ) @@ -413,12 +372,12 @@ def _spawn_sensors(self): 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) + time.sleep(0.001) return { 'image': self.image_queue.get(), 'lidar_distances': self.lidar_queue.get(), @@ -439,33 +398,22 @@ def get_obstacle_directions(self, lidar_distances): } def step(self, action=None): - """环境交互步骤（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) + """环境交互步骤""" + self.world.tick() # 清理无效NPC self.npc_vehicles = [npc for npc in self.npc_vehicles if self._is_actor_alive(npc)] - # 打印NPC速度（调试用） - if random.random() < 0.1: # 10%概率打印 + # 打印NPC速度（调试用，降低频率） + if random.random() < 0.02: 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 + speed = np.linalg.norm([velocity.x, velocity.y, velocity.z]) * 3.6 print(f"[NPC速度] ID:{npc.id} 速度:{speed:.1f}km/h") except Exception: pass - # 获取观测 observation = self.get_observation() reward = 1.0 done = False @@ -473,25 +421,19 @@ def step(self, action=None): def close(self): """清理资源""" - # 停止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]: self._safe_destroy_actor(sensor) - # 销毁NPC for npc in self.npc_vehicles: 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() @@ -499,30 +441,196 @@ def close(self): if self.settings: try: self.settings.synchronous_mode = False + self.settings.substepping = False self.world.apply_settings(self.settings) except Exception as e: print(f"[世界设置恢复警告] {str(e)}") print("[资源清理] 所有资源已销毁") + def init_spectator_smoother(self, window_size=15): + """初始化镜头平滑器（增大滑动窗口到15帧）""" + self.vehicle_pose_buffer = deque(maxlen=window_size) # 15帧缓存，过滤更多高频抖动 + self.window_size = window_size + # 初始化加权平均权重（近期帧权重更高，兼顾平滑和响应） + self.weights = np.linspace(0.1, 1.0, window_size) # 权重从0.1→1.0递增 + self.weights /= np.sum(self.weights) # 归一化 + + def update_spectator_ultra_smooth(self, spectator): + """ + 终极平滑镜头更新：加权滑动平均+微米级死区+完全锁定旋转 + """ + if not self._is_actor_alive(self.vehicle): + return + + # 1. 获取同步帧快照的车辆位姿（仅用快照，杜绝异步） + snapshot = self.world.get_snapshot() + vehicle_snapshot = snapshot.find(self.vehicle.id) + if not vehicle_snapshot: + return + current_pose = vehicle_snapshot.get_transform() + + # 2. 初始化关键变量 + avg_yaw = current_pose.rotation.yaw + avg_pose = current_pose + + # 3. 加入滑动缓存并计算「加权平均」位姿（核心优化） + self.vehicle_pose_buffer.append(current_pose) + if len(self.vehicle_pose_buffer) >= self.window_size: + # 提取缓存中的位姿 + poses = list(self.vehicle_pose_buffer) + count = len(poses) + + # 加权平均位置（近期帧权重更高） + avg_loc = carla.Location() + for i in range(count): + weight = self.weights[i] + avg_loc.x += poses[i].location.x * weight + avg_loc.y += poses[i].location.y * weight + avg_loc.z += poses[i].location.z * weight # Z轴也加权平均 + + # 加权平均Yaw角（处理360度环绕） + yaws = [pose.rotation.yaw for pose in poses] + yaw_rads = np.radians(yaws) + # 加权正弦和余弦 + weighted_sin = np.sum(np.sin(yaw_rads) * self.weights[:count]) + weighted_cos = np.sum(np.cos(yaw_rads) * self.weights[:count]) + avg_yaw = np.degrees(np.arctan2(weighted_sin, weighted_cos)) + + # 构建平均位姿 + avg_pose = carla.Transform(avg_loc, carla.Rotation(pitch=0, yaw=avg_yaw, roll=0)) + + # 4. 模拟父级绑定+Z轴强制锁定 + relative_loc = carla.Location(x=-5.0, y=0.0, z=2.0) + target_loc = avg_pose.transform(relative_loc) + target_loc.z = avg_pose.location.z + 2.0 # 强制锁定Z轴，不随任何波动 + + # 5. 镜头旋转：完全锁定（仅Yaw跟随平均位姿） + target_rot = carla.Rotation( + pitch=-10.0, # 完全固定，不参与任何平滑 + yaw=avg_yaw, + roll=0.0 # 完全固定 + ) + + # 6. EMA平滑+微米级死区过滤（终极去抖） + current_transform = spectator.get_transform() + alpha = 0.03 # 极致平滑系数（更小，更稳定） + pos_deadzone = 0.005 # 微米级死区（5mm内波动不更新） + + # 位置平滑（带死区） + loc_diff = np.array([ + target_loc.x - current_transform.location.x, + target_loc.y - current_transform.location.y, + target_loc.z - current_transform.location.z + ]) + loc_diff_mag = np.linalg.norm(loc_diff) + + if loc_diff_mag > pos_deadzone: + final_loc = carla.Location( + x=alpha * target_loc.x + (1 - alpha) * current_transform.location.x, + y=alpha * target_loc.y + (1 - alpha) * current_transform.location.y, + z=target_loc.z # Z轴直接锁定 + ) + else: + # 死区内不更新，保持当前位置 + final_loc = current_transform.location + + # 旋转平滑（仅Yaw，带死区） + yaw_diff = target_rot.yaw - current_transform.rotation.yaw + yaw_diff = (yaw_diff + 180) % 360 - 180 # 归一化 + rot_deadzone = 0.02 # 0.02度死区 + + if abs(yaw_diff) > rot_deadzone: + final_yaw = current_transform.rotation.yaw + alpha * yaw_diff + final_yaw = final_yaw % 360 + else: + final_yaw = current_transform.rotation.yaw + + final_rot = carla.Rotation( + pitch=-10.0, # 完全固定 + yaw=final_yaw, + roll=0.0 # 完全固定 + ) + + # 7. 更新镜头（仅一次，同步帧内完成） + spectator.set_transform(carla.Transform(final_loc, final_rot)) + -if __name__ == "__main__": - # 测试环境 +def run_simulation(): + pygame.init() + env = None try: + print("\n[CARLA连接] 创建环境...") 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("测试完成") + + # 初始化镜头平滑器（15帧加权滑动窗口） + env.init_spectator_smoother(window_size=15) + + print("\n[环境重置] 生成车辆和传感器...") + env.reset() + + if not env.vehicle or not env.vehicle.is_alive: + raise RuntimeError("车辆生成失败，请检查CARLA是否正常运行") + print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}），已启用自动驾驶") + + clock = pygame.time.Clock() + spectator = env.world.get_spectator() + + # 初始化镜头+填充缓存（延长初始化时间） + env.world.tick() + for _ in range(env.window_size * 2): # 填充2倍窗口，确保加权平均生效 + env.update_spectator_ultra_smooth(spectator) + env.world.tick() + + print("\n[仿真开始] 车辆将沿车道行驶，按Ctrl+C退出...") + sys.stdout.flush() + + step = 0 + obstacle_distances = {'front': 0, 'rear': 0, 'left': 0, 'right': 0} + while True: + # 处理pygame事件 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + raise KeyboardInterrupt + + # 严格同步的仿真帧推进 + env.world.tick() + + # 终极平滑的镜头更新 + env.update_spectator_ultra_smooth(spectator) + + # 极低频率获取观测（减少性能消耗） + if step % 3 == 0: + observation = env.get_observation() + obstacle_distances = env.get_obstacle_directions(observation['lidar_distances']) + + # 极低频率打印（每2秒打印一次，减少IO抖动） + if step % 120 == 0: + 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() + + # 锁定渲染帧率（与仿真帧率一致，避免波动） + clock.tick_busy_loop(60) # 更精准的帧率锁定 + step += 1 + + except KeyboardInterrupt: + print("\n[用户终止] 收到退出信号") except Exception as e: - print(f"测试出错：{str(e)}") - if 'env' in locals(): + print(f"\n[仿真错误] {str(e)}") + import traceback + traceback.print_exc() + sys.stdout.flush() + finally: + if env is not None: + print("\n[资源清理] 销毁资源...") env.close() - sys.exit(1) \ No newline at end of file + pygame.quit() + print("\n[程序退出]") + +if __name__ == "__main__": + print("="*60) + print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}") + print(f"[Python解释器] {sys.executable}") + print("="*60) + sys.stdout.flush() + run_simulation() \ 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 deleted file mode 100644 index 312538d685..0000000000 --- a/src/self_driving_car_navigation/run_simulation.py +++ /dev/null @@ -1,121 +0,0 @@ -import torch -import time -import sys -import pygame -import carla -import os -import sys - -# 添加当前目录到Python路径，确保可以导入_agent模块 -sys.path.append(os.path.dirname(os.path.abspath(__file__))) - -# 从_agent子目录导入CarlaEnvironment -from _agent.carla_environment import CarlaEnvironment - -print("="*60) -print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}") -print(f"[Python解释器] {sys.executable}") -print("="*60) -sys.stdout.flush() - -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 - ) - - if last_transform is None: - spectator.set_transform(target_tf) - return target_tf - - # 线性插值平滑过渡 - def lerp(a, b, t): - return a + t * (b - a) - - 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(): - # 初始化pygame - pygame.init() - - env = None - try: - print("\n[CARLA连接] 创建环境...") - env = CarlaEnvironment() - - print("\n[环境重置] 生成车辆和传感器...") - env.reset() - - if not env.vehicle or not env.vehicle.is_alive: - raise RuntimeError("车辆生成失败，请检查CARLA是否正常运行") - print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}），已启用自动驾驶") - - # 初始化平滑视角 - last_spectator_tf = set_spectator_smooth(env.world, env.vehicle) - print("视角已切换至车辆后上方（平滑跟随模式）") - - # 初始化时钟控制帧率 - clock = pygame.time.Clock() - - print("\n[仿真开始] 车辆将沿车道行驶，按Ctrl+C退出...") - sys.stdout.flush() - - # 持续运行仿真（不限制步数） - step = 0 - while True: - # 处理pygame事件 - for event in pygame.event.get(): - if event.type == pygame.QUIT: - raise KeyboardInterrupt - - # 同步CARLA帧（关键优化：保证控制时序稳定） - env.world.tick() - - # 获取观测和障碍物信息 - observation = env.get_observation() - 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() - finally: - if env is not None: - print("\n[资源清理] 销毁资源...") - env.close() - # 退出pygame - pygame.quit() - print("\n[程序退出]") - -if __name__ == "__main__": - run_simulation() \ No newline at end of file From 463a2b5edcb4d75eec78e9cf1098df8bc869bbc4 Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Wed, 24 Dec 2025 19:31:56 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E6=BF=80=E5=85=89?= =?UTF-8?q?=E9=9B=B7=E8=BE=BE3D=E7=82=B9=E4=BA=91=E5=8F=AF=E8=A7=86?= =?UTF-8?q?=E5=8C=96=EF=BC=88=E9=A2=9C=E8=89=B2=E7=BC=96=E7=A0=81=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/self_driving_car_navigation/run_carla.py | 179 ++++++++++++++++++- 1 file changed, 170 insertions(+), 9 deletions(-) diff --git a/src/self_driving_car_navigation/run_carla.py b/src/self_driving_car_navigation/run_carla.py index 480ac95f99..b82673b995 100644 --- a/src/self_driving_car_navigation/run_carla.py +++ b/src/self_driving_car_navigation/run_carla.py @@ -9,7 +9,9 @@ from queue import Queue from gym import spaces from collections import deque +import math +# ========== 原有CarlaEnvironment类保持不变 ========== class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() @@ -32,6 +34,8 @@ def __init__(self): self.image_queue = Queue(maxsize=1) self.lidar_queue = Queue(maxsize=1) self.imu_queue = Queue(maxsize=1) + # 新增：保存原始激光雷达点云数据（用于3D可视化） + self.raw_lidar_queue = Queue(maxsize=1) # 连接CARLA（强化严格同步模式） self._connect_carla() @@ -187,13 +191,20 @@ def process_image(self, image): print(f"[图像处理错误] {str(e)}") def process_lidar(self, data): - """处理激光雷达数据""" + """处理激光雷达数据（新增：保存原始点云用于3D可视化）""" try: if not hasattr(data, 'raw_data'): print("[激光雷达处理错误] 无效的激光雷达数据") return - points = np.frombuffer(data.raw_data, dtype=np.dtype('f4')).reshape(-1, 4)[:, :3] + # 原始点云（x,y,z,intensity）- 用于3D可视化 + raw_points = np.frombuffer(data.raw_data, dtype=np.dtype('f4')).reshape(-1, 4) + if self.raw_lidar_queue.full(): + self.raw_lidar_queue.get() + self.raw_lidar_queue.put(raw_points) + + # 原有2D距离计算逻辑 + points = raw_points[:, :3] distances = np.linalg.norm(points, axis=1) angles = np.arctan2(points[:, 1], points[:, 0]) * 180 / np.pi angles = (angles + 360) % 360 @@ -384,6 +395,12 @@ def get_observation(self): 'imu': self.imu_queue.get() } + def get_raw_lidar_points(self): + """获取原始激光雷达点云（用于3D可视化）""" + while self.raw_lidar_queue.empty(): + time.sleep(0.001) + return self.raw_lidar_queue.get() + def get_obstacle_directions(self, lidar_distances): """计算四向障碍物距离""" front_angles = np.concatenate([np.arange(345, 360), np.arange(0, 16)]) @@ -434,7 +451,7 @@ def close(self): 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]: + for q in [self.image_queue, self.lidar_queue, self.imu_queue, self.raw_lidar_queue]: while not q.empty(): q.get() # 恢复世界设置 @@ -554,10 +571,143 @@ def update_spectator_ultra_smooth(self, spectator): # 7. 更新镜头（仅一次，同步帧内完成） spectator.set_transform(carla.Transform(final_loc, final_rot)) +# ========== 新增：3D点云可视化核心逻辑 ========== +class Lidar3DVisualizer: + def __init__(self, width=600, height=600): + # 窗口参数 + self.width = width + self.height = height + self.screen = pygame.display.set_mode((width, height), pygame.DOUBLEBUF) + pygame.display.set_caption("激光雷达3D点云可视化") + + # 视角参数（默认从车辆后上方俯视） + self.theta = math.radians(30) # 水平旋转角（绕Y轴） + self.phi = math.radians(-30) # 垂直旋转角（绕X轴） + self.scale = 10.0 # 缩放系数 + self.offset_x = width // 2 + self.offset_y = height // 2 + + # 交互状态 + self.mouse_down = False + self.last_mouse_pos = (0, 0) + self.point_size = 2 # 点云渲染大小 + + # 颜色映射（距离→RGB） + self.color_map = { + 'red': (255, 0, 0), # 0-10m（近距/高风险） + 'yellow': (255, 255, 0),# 10-30m（中距） + 'green': (0, 255, 0) # 30-50m（远距/低风险） + } + def project_3d_to_2d(self, point): + """ + 透视投影：将3D点(x,y,z)转换为2D屏幕坐标 + 核心算法： + 1. 绕Y轴旋转（水平视角） + 2. 绕X轴旋转（垂直视角） + 3. 透视缩放+屏幕偏移 + """ + x, y, z = point + + # 绕Y轴旋转（theta） + x_rot = x * math.cos(self.theta) - z * math.sin(self.theta) + z_rot = x * math.sin(self.theta) + z * math.cos(self.theta) + + # 绕X轴旋转（phi） + y_rot = y * math.cos(self.phi) - z_rot * math.sin(self.phi) + z_rot = y * math.sin(self.phi) + z_rot * math.cos(self.phi) + + # 透视投影 + 屏幕偏移 + if z_rot == 0: + z_rot = 0.001 # 避免除零 + proj_x = self.offset_x + (x_rot * self.scale) / (z_rot / 5) + proj_y = self.offset_y - (y_rot * self.scale) / (z_rot / 5) + + return int(proj_x), int(proj_y) + + def get_point_color(self, distance): + """根据距离获取点云颜色（颜色编码）""" + if distance < 10: + return self.color_map['red'] + elif distance < 30: + return self.color_map['yellow'] + else: + return self.color_map['green'] + + def filter_invalid_points(self, points): + """过滤无效点：距离>50m/ <0.5m、z轴异常（地面/天空噪点）""" + # 计算每个点的距离 + distances = np.linalg.norm(points[:, :3], axis=1) + # 过滤条件 + mask = (distances > 0.5) & (distances < 50) & (points[:, 2] > -1) & (points[:, 2] < 5) + return points[mask], distances[mask] + + def render(self, raw_points): + """渲染3D点云""" + # 1. 清空屏幕（深灰背景，高级感） + self.screen.fill((20, 20, 20)) + + # 2. 过滤无效点 + valid_points, distances = self.filter_invalid_points(raw_points) + + # 3. 逐点投影并渲染 + for point, dist in zip(valid_points, distances): + x, y, z = point[:3] + # 转换为车辆局部坐标系（点云以车辆为中心） + local_point = (-x, -y, z) # 反转x/y，让点云朝向正确 + # 3D→2D投影 + screen_x, screen_y = self.project_3d_to_2d(local_point) + # 边界检查（避免渲染到窗口外） + if 0 < screen_x < self.width and 0 < screen_y < self.height: + color = self.get_point_color(dist) + # 绘制点云（抗锯齿） + pygame.draw.circle(self.screen, color, (screen_x, screen_y), self.point_size) + + # 4. 绘制辅助信息（坐标系、说明文字） + font = pygame.font.SysFont('Consolas', 12) + # 坐标系提示 + axis_text = font.render("X:前 Y:左 Z:上 | 鼠标拖拽旋转 | 滚轮缩放", True, (200, 200, 200)) + self.screen.blit(axis_text, (10, 10)) + # 距离说明 + dist_text = font.render("红色:<10m 黄色:10-30m 绿色:>30m", True, (200, 200, 200)) + self.screen.blit(dist_text, (10, 30)) + + # 5. 更新显示 + pygame.display.flip() + + def handle_events(self): + """处理鼠标交互""" + for event in pygame.event.get(): + # 鼠标按下 + if event.type == pygame.MOUSEBUTTONDOWN: + if event.button == 1: # 左键 + self.mouse_down = True + self.last_mouse_pos = pygame.mouse.get_pos() + elif event.button == 4: # 滚轮上滚（放大） + self.scale = min(self.scale + 1, 30) + elif event.button == 5: # 滚轮下滚（缩小） + self.scale = max(self.scale - 1, 5) + # 鼠标松开 + elif event.type == pygame.MOUSEBUTTONUP: + if event.button == 1: + self.mouse_down = False + # 鼠标拖拽（旋转视角） + elif event.type == pygame.MOUSEMOTION and self.mouse_down: + current_pos = pygame.mouse.get_pos() + dx = current_pos[0] - self.last_mouse_pos[0] + dy = current_pos[1] - self.last_mouse_pos[1] + # 更新旋转角（灵敏度适配） + self.theta += dx * 0.01 + self.phi += dy * 0.01 + self.last_mouse_pos = current_pos + +# ========== 修改后的仿真运行函数 ========== def run_simulation(): pygame.init() + # 初始化字体（避免中文/特殊字符乱码） + pygame.font.init() env = None + lidar_visualizer = None try: print("\n[CARLA连接] 创建环境...") env = CarlaEnvironment() @@ -572,6 +722,9 @@ def run_simulation(): raise RuntimeError("车辆生成失败，请检查CARLA是否正常运行") print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}），已启用自动驾驶") + # 初始化3D点云可视化器 + lidar_visualizer = Lidar3DVisualizer(width=600, height=600) + clock = pygame.time.Clock() spectator = env.world.get_spectator() @@ -582,34 +735,42 @@ def run_simulation(): env.world.tick() print("\n[仿真开始] 车辆将沿车道行驶，按Ctrl+C退出...") + print("[点云可视化] 窗口已启动 - 鼠标拖拽旋转视角 | 滚轮缩放 | 颜色编码距离") sys.stdout.flush() step = 0 obstacle_distances = {'front': 0, 'rear': 0, 'left': 0, 'right': 0} while True: - # 处理pygame事件 + # 1. 处理所有事件（主窗口+点云窗口） for event in pygame.event.get(): if event.type == pygame.QUIT: raise KeyboardInterrupt + # 点云窗口交互 + if lidar_visualizer: + lidar_visualizer.handle_events() - # 严格同步的仿真帧推进 + # 2. 严格同步的仿真帧推进 env.world.tick() - # 终极平滑的镜头更新 + # 3. 终极平滑的镜头更新 env.update_spectator_ultra_smooth(spectator) - # 极低频率获取观测（减少性能消耗） + # 4. 渲染3D点云（每帧更新，保证实时性） + raw_lidar_points = env.get_raw_lidar_points() + lidar_visualizer.render(raw_lidar_points) + + # 5. 极低频率获取观测（减少性能消耗） if step % 3 == 0: observation = env.get_observation() obstacle_distances = env.get_obstacle_directions(observation['lidar_distances']) - # 极低频率打印（每2秒打印一次，减少IO抖动） + # 6. 极低频率打印（每2秒打印一次，减少IO抖动） if step % 120 == 0: 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() - # 锁定渲染帧率（与仿真帧率一致，避免波动） + # 7. 锁定渲染帧率（与仿真帧率一致，避免波动） clock.tick_busy_loop(60) # 更精准的帧率锁定 step += 1 From fc16257bec27b582050959ed516820fd4fd1ad32 Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Wed, 24 Dec 2025 23:01:35 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E6=96=B0=E5=A2=9ECARLA=E4=BB=BF=E7=9C=9F3D?= =?UTF-8?q?=E8=A7=86=E8=A7=92=E5=A2=9E=E5=BC=BA+=E6=B8=85=E6=99=B0?= =?UTF-8?q?=E7=95=8C=E9=9D=A2=E4=BA=A4=E4=BA=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/self_driving_car_navigation/run_carla.py | 481 +++++++++++++------ 1 file changed, 322 insertions(+), 159 deletions(-) diff --git a/src/self_driving_car_navigation/run_carla.py b/src/self_driving_car_navigation/run_carla.py index b82673b995..e4bd574926 100644 --- a/src/self_driving_car_navigation/run_carla.py +++ b/src/self_driving_car_navigation/run_carla.py @@ -11,7 +11,7 @@ from collections import deque import math -# ========== 原有CarlaEnvironment类保持不变 ========== +# ========== 原有CarlaEnvironment类（重点修改视角平滑逻辑） ========== class CarlaEnvironment(gym.Env): def __init__(self): super(CarlaEnvironment, self).__init__() @@ -34,8 +34,17 @@ def __init__(self): self.image_queue = Queue(maxsize=1) self.lidar_queue = Queue(maxsize=1) self.imu_queue = Queue(maxsize=1) - # 新增：保存原始激光雷达点云数据（用于3D可视化） + # 激光雷达3D点云专用队列 self.raw_lidar_queue = Queue(maxsize=1) + # 新增：当前视角模式（默认第三人称） + self.current_view_mode = "third_person" # first_person / third_person / bird_view + + # ========== 新增：视角平滑相关变量（解决抖动核心） ========== + self.smooth_pos = None # 平滑后的位置缓存 + self.smooth_rot = None # 平滑后的旋转缓存 + self.pos_smooth_factor = 0.05 # 位置平滑系数（0.05~0.2，越小越平滑） + self.rot_smooth_factor = 0.08 # 旋转平滑系数（比位置更平滑） + self.view_switch_flag = False # 视角切换标记（切换时跳过平滑） # 连接CARLA（强化严格同步模式） self._connect_carla() @@ -191,7 +200,7 @@ def process_image(self, image): print(f"[图像处理错误] {str(e)}") def process_lidar(self, data): - """处理激光雷达数据（新增：保存原始点云用于3D可视化）""" + """处理激光雷达数据（保留原始点云用于3D可视化）""" try: if not hasattr(data, 'raw_data'): print("[激光雷达处理错误] 无效的激光雷达数据") @@ -254,6 +263,11 @@ def reset(self): self.vehicle.set_autopilot(True, self.tm_port) self._spawn_npcs(60) self._clear_all_non_ego_actors() + # 重置视角模式和平滑缓存 + self.current_view_mode = "third_person" + self.smooth_pos = None + self.smooth_rot = None + self.view_switch_flag = False # 多次同步，物理稳定（延长时间） for _ in range(15): @@ -356,13 +370,16 @@ def _spawn_sensors(self): 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') + # 第一人称视角摄像头（驾驶位） self.camera = self.world.spawn_actor( - camera_bp, carla.Transform(carla.Location(x=2.0, z=1.5)), attach_to=self.vehicle + camera_bp, carla.Transform(carla.Location(x=1.0, y=0.0, z=1.2), carla.Rotation(pitch=0, yaw=0, roll=0)), + attach_to=self.vehicle, + attachment_type=carla.AttachmentType.Rigid ) self.camera.listen(self.process_image) @@ -464,114 +481,69 @@ def close(self): print(f"[世界设置恢复警告] {str(e)}") print("[资源清理] 所有资源已销毁") - def init_spectator_smoother(self, window_size=15): - """初始化镜头平滑器（增大滑动窗口到15帧）""" - self.vehicle_pose_buffer = deque(maxlen=window_size) # 15帧缓存，过滤更多高频抖动 - self.window_size = window_size - # 初始化加权平均权重（近期帧权重更高，兼顾平滑和响应） - self.weights = np.linspace(0.1, 1.0, window_size) # 权重从0.1→1.0递增 - self.weights /= np.sum(self.weights) # 归一化 - - def update_spectator_ultra_smooth(self, spectator): - """ - 终极平滑镜头更新：加权滑动平均+微米级死区+完全锁定旋转 - """ + # ========== 核心修改：视角切换+平滑跟随（解决抖动） ========== + def switch_view_mode(self, mode): + """切换视角模式（添加切换标记，跳过初始平滑）""" + if mode in ["first_person", "third_person", "bird_view"]: + self.current_view_mode = mode + self.view_switch_flag = True # 切换时强制重置平滑缓存 + print(f"[视角切换] 已切换至：{self.current_view_mode}") + + def _smooth_transform(self, target_loc, target_rot): + """对位置和旋转进行指数移动平均平滑（核心防抖逻辑）""" + # 首次初始化平滑缓存 + if self.smooth_pos is None or self.view_switch_flag: + self.smooth_pos = target_loc + self.smooth_rot = target_rot + self.view_switch_flag = False # 重置切换标记 + return self.smooth_pos, self.smooth_rot + + # 位置平滑（指数移动平均） + self.smooth_pos.x = self.smooth_pos.x * (1 - self.pos_smooth_factor) + target_loc.x * self.pos_smooth_factor + self.smooth_pos.y = self.smooth_pos.y * (1 - self.pos_smooth_factor) + target_loc.y * self.pos_smooth_factor + self.smooth_pos.z = self.smooth_pos.z * (1 - self.pos_smooth_factor) + target_loc.z * self.pos_smooth_factor + + # 旋转平滑（指数移动平均，仅yaw角跟随，pitch/roll固定） + self.smooth_rot.yaw = self.smooth_rot.yaw * (1 - self.rot_smooth_factor) + target_rot.yaw * self.rot_smooth_factor + self.smooth_rot.pitch = target_rot.pitch # 俯仰角固定，不平滑 + self.smooth_rot.roll = target_rot.roll # 滚转角固定，不平滑 + + return self.smooth_pos, self.smooth_rot + + def update_view(self, spectator): + """根据当前视角模式更新 spectator 位置（添加平滑防抖）""" if not self._is_actor_alive(self.vehicle): return - # 1. 获取同步帧快照的车辆位姿（仅用快照，杜绝异步） - snapshot = self.world.get_snapshot() - vehicle_snapshot = snapshot.find(self.vehicle.id) - if not vehicle_snapshot: - return - current_pose = vehicle_snapshot.get_transform() - - # 2. 初始化关键变量 - avg_yaw = current_pose.rotation.yaw - avg_pose = current_pose - - # 3. 加入滑动缓存并计算「加权平均」位姿（核心优化） - self.vehicle_pose_buffer.append(current_pose) - if len(self.vehicle_pose_buffer) >= self.window_size: - # 提取缓存中的位姿 - poses = list(self.vehicle_pose_buffer) - count = len(poses) - - # 加权平均位置（近期帧权重更高） - avg_loc = carla.Location() - for i in range(count): - weight = self.weights[i] - avg_loc.x += poses[i].location.x * weight - avg_loc.y += poses[i].location.y * weight - avg_loc.z += poses[i].location.z * weight # Z轴也加权平均 - - # 加权平均Yaw角（处理360度环绕） - yaws = [pose.rotation.yaw for pose in poses] - yaw_rads = np.radians(yaws) - # 加权正弦和余弦 - weighted_sin = np.sum(np.sin(yaw_rads) * self.weights[:count]) - weighted_cos = np.sum(np.cos(yaw_rads) * self.weights[:count]) - avg_yaw = np.degrees(np.arctan2(weighted_sin, weighted_cos)) - - # 构建平均位姿 - avg_pose = carla.Transform(avg_loc, carla.Rotation(pitch=0, yaw=avg_yaw, roll=0)) - - # 4. 模拟父级绑定+Z轴强制锁定 - relative_loc = carla.Location(x=-5.0, y=0.0, z=2.0) - target_loc = avg_pose.transform(relative_loc) - target_loc.z = avg_pose.location.z + 2.0 # 强制锁定Z轴，不随任何波动 - - # 5. 镜头旋转：完全锁定（仅Yaw跟随平均位姿） - target_rot = carla.Rotation( - pitch=-10.0, # 完全固定，不参与任何平滑 - yaw=avg_yaw, - roll=0.0 # 完全固定 - ) - - # 6. EMA平滑+微米级死区过滤（终极去抖） - current_transform = spectator.get_transform() - alpha = 0.03 # 极致平滑系数（更小，更稳定） - pos_deadzone = 0.005 # 微米级死区（5mm内波动不更新） - - # 位置平滑（带死区） - loc_diff = np.array([ - target_loc.x - current_transform.location.x, - target_loc.y - current_transform.location.y, - target_loc.z - current_transform.location.z - ]) - loc_diff_mag = np.linalg.norm(loc_diff) - - if loc_diff_mag > pos_deadzone: - final_loc = carla.Location( - x=alpha * target_loc.x + (1 - alpha) * current_transform.location.x, - y=alpha * target_loc.y + (1 - alpha) * current_transform.location.y, - z=target_loc.z # Z轴直接锁定 - ) - else: - # 死区内不更新，保持当前位置 - final_loc = current_transform.location - - # 旋转平滑（仅Yaw，带死区） - yaw_diff = target_rot.yaw - current_transform.rotation.yaw - yaw_diff = (yaw_diff + 180) % 360 - 180 # 归一化 - rot_deadzone = 0.02 # 0.02度死区 - - if abs(yaw_diff) > rot_deadzone: - final_yaw = current_transform.rotation.yaw + alpha * yaw_diff - final_yaw = final_yaw % 360 + vehicle_transform = self.vehicle.get_transform() + vehicle_location = vehicle_transform.location + vehicle_rotation = vehicle_transform.rotation + + # 计算目标视角位置和旋转 + if self.current_view_mode == "first_person": + # 第一人称视角：驾驶位，向前看 + target_loc = vehicle_transform.transform(carla.Location(x=0.5, y=0.0, z=1.0)) + target_rot = vehicle_rotation + elif self.current_view_mode == "third_person": + # 第三人称视角：车辆后上方，平滑跟随 + target_loc = vehicle_transform.transform(carla.Location(x=-5.0, y=0.0, z=2.0)) + target_rot = carla.Rotation(pitch=-10.0, yaw=vehicle_rotation.yaw, roll=0.0) + elif self.current_view_mode == "bird_view": + # 鸟瞰视角：车辆正上方，全局俯视 + target_loc = carla.Location(vehicle_location.x, vehicle_location.y, vehicle_location.z + 50.0) + target_rot = carla.Rotation(pitch=-90.0, yaw=vehicle_rotation.yaw, roll=0.0) else: - final_yaw = current_transform.rotation.yaw + # 默认第三人称 + target_loc = vehicle_transform.transform(carla.Location(x=-5.0, y=0.0, z=2.0)) + target_rot = carla.Rotation(pitch=-10.0, yaw=vehicle_rotation.yaw, roll=0.0) - final_rot = carla.Rotation( - pitch=-10.0, # 完全固定 - yaw=final_yaw, - roll=0.0 # 完全固定 - ) + # 对目标视角进行平滑处理（核心防抖） + smooth_loc, smooth_rot = self._smooth_transform(target_loc, target_rot) - # 7. 更新镜头（仅一次，同步帧内完成） - spectator.set_transform(carla.Transform(final_loc, final_rot)) + # 更新 spectator（使用平滑后的位置和旋转） + spectator.set_transform(carla.Transform(smooth_loc, smooth_rot)) -# ========== 新增：3D点云可视化核心逻辑 ========== +# ========== 激光雷达3D点云可视化窗口（保留，单独运行） ========== class Lidar3DVisualizer: def __init__(self, width=600, height=600): # 窗口参数 @@ -587,7 +559,7 @@ def __init__(self, width=600, height=600): self.offset_x = width // 2 self.offset_y = height // 2 - # 交互状态 + # 交互状态（即使暂未生效，保留逻辑） self.mouse_down = False self.last_mouse_pos = (0, 0) self.point_size = 2 # 点云渲染大小 @@ -600,13 +572,7 @@ def __init__(self, width=600, height=600): } def project_3d_to_2d(self, point): - """ - 透视投影：将3D点(x,y,z)转换为2D屏幕坐标 - 核心算法： - 1. 绕Y轴旋转（水平视角） - 2. 绕X轴旋转（垂直视角） - 3. 透视缩放+屏幕偏移 - """ + """透视投影：将3D点(x,y,z)转换为2D屏幕坐标""" x, y, z = point # 绕Y轴旋转（theta） @@ -664,9 +630,9 @@ def render(self, raw_points): pygame.draw.circle(self.screen, color, (screen_x, screen_y), self.point_size) # 4. 绘制辅助信息（坐标系、说明文字） - font = pygame.font.SysFont('Consolas', 12) + font = pygame.font.SysFont('Consolas', 12, bold=True) # 坐标系提示 - axis_text = font.render("X:前 Y:左 Z:上 | 鼠标拖拽旋转 | 滚轮缩放", True, (200, 200, 200)) + axis_text = font.render("X:前 Y:左 Z:上 | 激光雷达有效范围：0.5-50m", True, (200, 200, 200)) self.screen.blit(axis_text, (10, 10)) # 距离说明 dist_text = font.render("红色:<10m 黄色:10-30m 绿色:>30m", True, (200, 200, 200)) @@ -676,7 +642,7 @@ def render(self, raw_points): pygame.display.flip() def handle_events(self): - """处理鼠标交互""" + """处理鼠标交互（保留逻辑，即使暂未生效）""" for event in pygame.event.get(): # 鼠标按下 if event.type == pygame.MOUSEBUTTONDOWN: @@ -701,20 +667,205 @@ def handle_events(self): self.phi += dy * 0.01 self.last_mouse_pos = current_pos -# ========== 修改后的仿真运行函数 ========== +# ========== 主可视化界面（3D视角增强+清晰交互） ========== +class MainVisualizer: + def __init__(self, width=800, height=600): + # 窗口基础配置 + self.width = width + self.height = height + self.screen = pygame.display.set_mode((width, height), pygame.DOUBLEBUF | pygame.HWSURFACE) + pygame.display.set_caption("CARLA自动驾驶仿真 - 3D视角控制") + + # 字体配置（确保显示清晰，抗锯齿） + self.font_large = pygame.font.SysFont('Microsoft YaHei', 16, bold=True) + self.font_small = pygame.font.SysFont('Microsoft YaHei', 12, bold=True) + + # 颜色配置（工业风，高对比度，显示清晰） + self.colors = { + 'bg_main': (18, 18, 28), # 主背景（深蓝灰） + 'bg_panel': (30, 30, 45), # 面板背景（深紫灰） + 'text_normal': (220, 220, 240),# 普通文字（浅灰蓝） + 'text_highlight': (0, 180, 255),# 高亮文字（天蓝色） + 'button_normal': (45, 45, 65), # 按钮常态 + 'button_hover': (60, 60, 85), # 按钮悬停 + 'button_press': (0, 120, 180), # 按钮按下 + 'border': (80, 80, 100) # 边框颜色 + } + + # 交互按钮配置（位置+大小+文本） + self.buttons = { + 'first_person': { + 'rect': pygame.Rect(20, 500, 120, 40), + 'text': '第一人称 (1)', + 'active': False + }, + 'third_person': { + 'rect': pygame.Rect(150, 500, 120, 40), + 'text': '第三人称 (2)', + 'active': True # 默认激活 + }, + 'bird_view': { + 'rect': pygame.Rect(280, 500, 120, 40), + 'text': '鸟瞰视角 (3)', + 'active': False + }, + 'pause': { + 'rect': pygame.Rect(410, 500, 120, 40), + 'text': '暂停 (空格)', + 'active': False + } + } + + # 状态变量 + self.paused = False + self.fps = 0 + self.npc_count = 0 + self.obstacle_distances = {'front': 0, 'rear': 0, 'left': 0, 'right': 0} + self.current_view = "第三人称" + + def draw_buttons(self): + """绘制交互按钮（清晰易识别，有状态反馈）""" + for btn_name, btn in self.buttons.items(): + # 确定按钮颜色（根据状态） + if btn['active']: + bg_color = self.colors['button_press'] + elif btn['rect'].collidepoint(pygame.mouse.get_pos()): + bg_color = self.colors['button_hover'] + else: + bg_color = self.colors['button_normal'] + + # 绘制按钮（带边框，清晰） + pygame.draw.rect(self.screen, bg_color, btn['rect'], border_radius=5) + pygame.draw.rect(self.screen, self.colors['border'], btn['rect'], width=2, border_radius=5) + + # 绘制按钮文字（居中，抗锯齿） + text_surf = self.font_small.render(btn['text'], True, self.colors['text_normal']) + text_rect = text_surf.get_rect(center=btn['rect'].center) + self.screen.blit(text_surf, text_rect) + + def draw_status_panel(self): + """绘制状态面板（信息清晰，分区显示）""" + # 面板背景 + panel_rect = pygame.Rect(20, 20, 300, 150) + pygame.draw.rect(self.screen, self.colors['bg_panel'], panel_rect, border_radius=5) + pygame.draw.rect(self.screen, self.colors['border'], panel_rect, width=2, border_radius=5) + + # 面板标题 + title_surf = self.font_large.render("仿真状态信息", True, self.colors['text_highlight']) + self.screen.blit(title_surf, (30, 30)) + + # 状态文本（分行显示，对齐） + status_texts = [ + f"当前视角：{self.current_view}", + f"仿真帧率：{self.fps:.1f} FPS", + f"NPC车辆数：{self.npc_count} 辆", + f"前方障碍物：{self.obstacle_distances['front']:.1f} m", + f"右侧障碍物：{self.obstacle_distances['right']:.1f} m" + ] + + for i, text in enumerate(status_texts): + y_pos = 60 + i * 20 + text_surf = self.font_small.render(text, True, self.colors['text_normal']) + self.screen.blit(text_surf, (30, y_pos)) + + def draw_help_text(self): + """绘制操作提示（清晰易读）""" + help_texts = [ + "快捷键：1-第一人称 2-第三人称 3-鸟瞰视角 空格-暂停/继续 ESC-退出", + "视角说明：第一人称（驾驶位）| 第三人称（跟随）| 鸟瞰（全局俯视）" + ] + for i, text in enumerate(help_texts): + y_pos = 450 + i * 20 + text_surf = self.font_small.render(text, True, self.colors['text_normal']) + self.screen.blit(text_surf, (20, y_pos)) + + def update_status(self, fps, npc_count, obstacle_distances, current_view): + """更新状态信息（用于渲染）""" + self.fps = fps + self.npc_count = npc_count + self.obstacle_distances = obstacle_distances + self.current_view = current_view + + # 更新按钮激活状态 + for btn_name in self.buttons: + self.buttons[btn_name]['active'] = False + if current_view == "第一人称": + self.buttons['first_person']['active'] = True + elif current_view == "第三人称": + self.buttons['third_person']['active'] = True + elif current_view == "鸟瞰视角": + self.buttons['bird_view']['active'] = True + self.buttons['pause']['active'] = self.paused + + def handle_events(self, env): + """处理交互事件（快捷键+按钮点击）""" + for event in pygame.event.get(): + # 窗口关闭 + if event.type == pygame.QUIT: + raise KeyboardInterrupt + # 键盘按键 + elif event.type == pygame.KEYDOWN: + if event.key == pygame.K_ESCAPE: + raise KeyboardInterrupt + elif event.key == pygame.K_1: + env.switch_view_mode("first_person") + self.current_view = "第一人称" + elif event.key == pygame.K_2: + env.switch_view_mode("third_person") + self.current_view = "第三人称" + elif event.key == pygame.K_3: + env.switch_view_mode("bird_view") + self.current_view = "鸟瞰视角" + elif event.key == pygame.K_SPACE: + self.paused = not self.paused + # 鼠标点击按钮 + elif event.type == pygame.MOUSEBUTTONDOWN: + if event.button == 1: + mouse_pos = pygame.mouse.get_pos() + if self.buttons['first_person']['rect'].collidepoint(mouse_pos): + env.switch_view_mode("first_person") + self.current_view = "第一人称" + elif self.buttons['third_person']['rect'].collidepoint(mouse_pos): + env.switch_view_mode("third_person") + self.current_view = "第三人称" + elif self.buttons['bird_view']['rect'].collidepoint(mouse_pos): + env.switch_view_mode("bird_view") + self.current_view = "鸟瞰视角" + elif self.buttons['pause']['rect'].collidepoint(mouse_pos): + self.paused = not self.paused + + def render(self): + """渲染主界面（所有元素）""" + # 清空主背景 + self.screen.fill(self.colors['bg_main']) + + # 绘制状态面板 + self.draw_status_panel() + + # 绘制操作提示 + self.draw_help_text() + + # 绘制交互按钮 + self.draw_buttons() + + # 更新显示（双缓冲，无撕裂） + pygame.display.flip() + +# ========== 仿真运行函数 ========== def run_simulation(): pygame.init() - # 初始化字体（避免中文/特殊字符乱码） + # 初始化字体（确保中文显示清晰，抗锯齿） pygame.font.init() + # 禁用pygame默认鼠标缩放，避免干扰 + pygame.mouse.set_cursor(pygame.SYSTEM_CURSOR_ARROW) + env = None lidar_visualizer = None + main_visualizer = None try: print("\n[CARLA连接] 创建环境...") env = CarlaEnvironment() - # 初始化镜头平滑器（15帧加权滑动窗口） - env.init_spectator_smoother(window_size=15) - print("\n[环境重置] 生成车辆和传感器...") env.reset() @@ -722,57 +873,68 @@ def run_simulation(): raise RuntimeError("车辆生成失败，请检查CARLA是否正常运行") print(f"[车辆状态] 生成成功（ID: {env.vehicle.id}），已启用自动驾驶") - # 初始化3D点云可视化器 + # 初始化两个可视化窗口 lidar_visualizer = Lidar3DVisualizer(width=600, height=600) + main_visualizer = MainVisualizer(width=800, height=600) clock = pygame.time.Clock() spectator = env.world.get_spectator() - # 初始化镜头+填充缓存（延长初始化时间） - env.world.tick() - for _ in range(env.window_size * 2): # 填充2倍窗口，确保加权平均生效 - env.update_spectator_ultra_smooth(spectator) - env.world.tick() + # 初始化视角（强制一次平滑缓存） + env.update_view(spectator) - print("\n[仿真开始] 车辆将沿车道行驶，按Ctrl+C退出...") - print("[点云可视化] 窗口已启动 - 鼠标拖拽旋转视角 | 滚轮缩放 | 颜色编码距离") + print("\n[仿真开始] 按ESC退出 | 快捷键1/2/3切换视角 | 空格暂停") + print("[窗口说明] 主窗口（视角控制）| 独立窗口（激光雷达3D点云）") sys.stdout.flush() step = 0 obstacle_distances = {'front': 0, 'rear': 0, 'left': 0, 'right': 0} while True: - # 1. 处理所有事件（主窗口+点云窗口） - for event in pygame.event.get(): - if event.type == pygame.QUIT: - raise KeyboardInterrupt - # 点云窗口交互 - if lidar_visualizer: - lidar_visualizer.handle_events() + # 1. 处理所有交互事件 + main_visualizer.handle_events(env) + lidar_visualizer.handle_events() - # 2. 严格同步的仿真帧推进 - env.world.tick() - - # 3. 终极平滑的镜头更新 - env.update_spectator_ultra_smooth(spectator) - - # 4. 渲染3D点云（每帧更新，保证实时性） - raw_lidar_points = env.get_raw_lidar_points() - lidar_visualizer.render(raw_lidar_points) - - # 5. 极低频率获取观测（减少性能消耗） - if step % 3 == 0: - observation = env.get_observation() - obstacle_distances = env.get_obstacle_directions(observation['lidar_distances']) + # 2. 暂停逻辑 + if not main_visualizer.paused: + # 严格同步的仿真帧推进 + env.world.tick() + + # 更新CARLA spectator视角（带平滑防抖） + env.update_view(spectator) + + # 获取观测数据（低频率，减少消耗） + if step % 3 == 0: + observation = env.get_observation() + obstacle_distances = env.get_obstacle_directions(observation['lidar_distances']) + # 渲染激光雷达3D点云 + raw_lidar_points = env.get_raw_lidar_points() + lidar_visualizer.render(raw_lidar_points) + + # 打印日志（极低频率） + if step % 120 == 0: + 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() + + step += 1 - # 6. 极低频率打印（每2秒打印一次，减少IO抖动） - if step % 120 == 0: - 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() + # 3. 更新主界面状态并渲染 + current_fps = clock.get_fps() + current_view_name = { + "first_person": "第一人称", + "third_person": "第三人称", + "bird_view": "鸟瞰视角" + }.get(env.current_view_mode, "第三人称") + main_visualizer.update_status( + fps=current_fps, + npc_count=len(env.npc_vehicles), + obstacle_distances=obstacle_distances, + current_view=current_view_name + ) + main_visualizer.render() - # 7. 锁定渲染帧率（与仿真帧率一致，避免波动） - clock.tick_busy_loop(60) # 更精准的帧率锁定 - step += 1 + # 4. 锁定渲染帧率（与仿真帧率一致） + clock.tick_busy_loop(60) except KeyboardInterrupt: print("\n[用户终止] 收到退出信号") @@ -792,6 +954,7 @@ def run_simulation(): print("="*60) print(f"[启动时间] {time.strftime('%Y-%m-%d %H:%M:%S')}") print(f"[Python解释器] {sys.executable}") + print(f"[运行环境] Win11 + CARLA0.9.11 + Python3.7.5") print("="*60) sys.stdout.flush() run_simulation() \ No newline at end of file