Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 45 additions & 0 deletions src/self_driving_car_navigation/ros/decision_module.py
Original file line number Diff line number Diff line change
@@ -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
50 changes: 50 additions & 0 deletions src/self_driving_car_navigation/ros/perception_module.py
Original file line number Diff line number Diff line change
@@ -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
40 changes: 40 additions & 0 deletions src/self_driving_car_navigation/ros/ros_test_node.py
Original file line number Diff line number Diff line change
@@ -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("节点已被中断退出。")
Loading