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/3] =?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/3] =?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/3] =?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}] 加载参数完成:")