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/4] =?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/4] =?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/4] =?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/4] =?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