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