From b86b125ee5c39f3d7ee597beb16aadb9a9757f45 Mon Sep 17 00:00:00 2001 From: Pan-j-l <43541417288@qq.com> Date: Thu, 18 Dec 2025 16:03:45 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90ROS=E8=8A=82=E7=82=B9?= =?UTF-8?q?=E5=B0=81=E8=A3=85=EF=BC=9A=E9=9B=86=E6=88=90=E6=84=9F=E7=9F=A5?= =?UTF-8?q?/=E5=86=B3=E7=AD=96=E6=A8=A1=E5=9D=97=E7=9A=84=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros/decision_module.py | 45 +++++++++++++++++ .../ros/perception_module.py | 50 +++++++++++++++++++ .../ros/ros_test_node.py | 40 +++++++++++++++ 3 files changed, 135 insertions(+) create mode 100644 src/self_driving_car_navigation/ros/decision_module.py create mode 100644 src/self_driving_car_navigation/ros/perception_module.py create mode 100644 src/self_driving_car_navigation/ros/ros_test_node.py diff --git a/src/self_driving_car_navigation/ros/decision_module.py b/src/self_driving_car_navigation/ros/decision_module.py new file mode 100644 index 0000000000..f5a8ab2359 --- /dev/null +++ b/src/self_driving_car_navigation/ros/decision_module.py @@ -0,0 +1,45 @@ +# scripts/models/decision_module.py +import torch + +class DecisionModule: + def __init__(self, + input_feature_dim=128, # 输入感知特征维度(与感知模块输出一致) + output_cmd_dim=2): # 输出控制指令维度(线速度x + 角速度z) + """初始化决策模块""" + self.feature_dim = input_feature_dim + self.cmd_dim = output_cmd_dim + + # 示例:初始化一个简单的决策层(可替换为你的实际模型,如DRL策略网络) + self.decision_layer = torch.nn.Sequential( + torch.nn.Linear(input_feature_dim, 64), + torch.nn.ReLU(), + torch.nn.Linear(64, output_cmd_dim) + ) + + # 控制指令范围(示例:线速度0~2m/s,角速度-1~1rad/s) + self.max_linear_vel = 2.0 + self.min_linear_vel = 0.0 + self.max_angular_vel = 1.0 + self.min_angular_vel = -1.0 + + def get_control_cmd(self, fused_feature): + """ + 接收感知特征,输出控制指令 + 参数: + fused_feature: torch.Tensor - 感知融合特征,shape=(B, 128) + 返回: + control_cmd: dict - 控制指令(线速度x、角速度z) + """ + # 1. 决策层推理 + raw_cmd = self.decision_layer(fused_feature) # (B, 2) + + # 2. 裁剪指令到合理范围 + linear_vel_x = torch.clamp(raw_cmd[:, 0], self.min_linear_vel, self.max_linear_vel).item() + angular_vel_z = torch.clamp(raw_cmd[:, 1], self.min_angular_vel, self.max_angular_vel).item() + + # 3. 包装为字典(便于ROS消息转换) + control_cmd = { + "linear_x": linear_vel_x, + "angular_z": angular_vel_z + } + return control_cmd \ No newline at end of file diff --git a/src/self_driving_car_navigation/ros/perception_module.py b/src/self_driving_car_navigation/ros/perception_module.py new file mode 100644 index 0000000000..0bfd65aee6 --- /dev/null +++ b/src/self_driving_car_navigation/ros/perception_module.py @@ -0,0 +1,50 @@ +# scripts/models/perception_module.py +import torch +import numpy as np +from cv_bridge import CvBridge # 若涉及图像转换,需提前安装:pip install cv-bridge + +class PerceptionModule: + def __init__(self, + image_input_shape=(3, 128, 128), # 图像输入维度:(通道数, 高, 宽) + lidar_input_dim=360, # LiDAR输入维度(360个距离点) + imu_input_dim=6, # IMU输入维度(加速度x/y/z + 角速度x/y/z) + output_feature_dim=128): # 输出感知特征维度 + """初始化感知模块""" + self.image_shape = image_input_shape + self.lidar_dim = lidar_input_dim + self.imu_dim = imu_input_dim + self.feature_dim = output_feature_dim + + # 示例:初始化一个简单的特征融合层(可替换为你的实际模型) + self.fusion_layer = torch.nn.Linear( + (image_input_shape[0]*image_input_shape[1]*image_input_shape[2]) + lidar_input_dim + imu_input_dim, + output_feature_dim + ) + self.bridge = CvBridge() # 图像格式转换工具(ROS Image ↔ OpenCV) + + def process(self, imu_data, image_data, lidar_data): + """ + 处理传感器数据,输出融合特征 + 参数: + imu_data: torch.Tensor - IMU数据,shape=(B, 6) + image_data: torch.Tensor - 图像数据,shape=(B, 3, 128, 128) + lidar_data: torch.Tensor - LiDAR数据,shape=(B, 360) + 返回: + fused_feature: torch.Tensor - 融合特征,shape=(B, 128) + """ + # 1. 扁平化各传感器数据 + image_flat = image_data.flatten(start_dim=1) # (B, 3*128*128) + lidar_flat = lidar_data.flatten(start_dim=1) # (B, 360) + imu_flat = imu_data.flatten(start_dim=1) # (B, 6) + + # 2. 拼接数据并融合 + combined_data = torch.cat([image_flat, lidar_flat, imu_flat], dim=1) + fused_feature = self.fusion_layer(combined_data) # 简单示例:全连接层融合 + + return fused_feature + + def ros_image_to_tensor(self, ros_image_msg): + """将ROS Image消息转换为模型输入的Tensor""" + cv_image = self.bridge.imgmsg_to_cv2(ros_image_msg, "bgr8") # ROS → OpenCV + tensor_image = torch.tensor(cv_image, dtype=torch.float32).permute(2, 0, 1).unsqueeze(0) # (1, 3, 128, 128) + return tensor_image \ No newline at end of file diff --git a/src/self_driving_car_navigation/ros/ros_test_node.py b/src/self_driving_car_navigation/ros/ros_test_node.py new file mode 100644 index 0000000000..9cf416ccb4 --- /dev/null +++ b/src/self_driving_car_navigation/ros/ros_test_node.py @@ -0,0 +1,40 @@ +#!/home/pan-j-l/my_ros_project/venv_py37/bin/python3 +import rospy +import torch +from models.perception_module import PerceptionModule +from models.decision_module import DecisionModule + +def main(): + rospy.init_node('ros_test_node', anonymous=True) + rospy.loginfo("测试节点启动成功!") + + try: + # 初始化感知/决策模块 + perception_model = PerceptionModule() + decision_model = DecisionModule() + rospy.loginfo("感知模块和决策模块初始化成功!") + + # 测试模块功能(模拟传感器数据) + mock_imu = torch.randn(1, 6) # 模拟1个batch的IMU数据 + mock_image = torch.randn(1, 3, 128, 128) # 模拟图像数据 + mock_lidar = torch.randn(1, 360) # 模拟LiDAR数据 + + # 感知模块处理 + fused_feature = perception_model.process(mock_imu, mock_image, mock_lidar) + rospy.loginfo(f"感知特征生成成功,shape: {fused_feature.shape}") + + # 决策模块输出控制指令 + control_cmd = decision_model.get_control_cmd(fused_feature) + rospy.loginfo(f"决策控制指令:线速度x={control_cmd['linear_x']:.2f}m/s,角速度z={control_cmd['angular_z']:.2f}rad/s") + + except Exception as e: + rospy.logerr(f"模块测试失败:{str(e)}") + return + + rospy.spin() + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + rospy.loginfo("节点已被中断退出。") \ No newline at end of file