哈哈哈哈哈哈哈哈哈哈，还是昨天那个程序，因为很少有Python关于ROS开发的相关文章，自己真的是各种尝试，最后在大佬的帮助下终于成功了。
上代码：
#!/usr/bin/env python #coding=utf-8 ''' ************************************************************ 作者：EnKant ************************************************************ ''' import rospy import os import numpy as np import math import cv2 from sensor_msgs.msg import PointCloud2, PointField from sensor_msgs import point_cloud2 ''' ************************************************************ 功能：四元数 转 旋转矩阵 输入：1×4的四元数向量 返回值：3×3的旋转矩阵 ************************************************************ ''' def q2R(q): q = np.outer(q, q) rot_matrix = np.transpose( [[1.0 - 2*q[2, 2] - 2*q[3, 3], 2*q[1, 2] + 2*q[0, 3] , 2*q[1, 3] - 2*q[0, 2]] , [2*q[1, 2] - 2*q[0, 3] , 1.0 - 2*q[1, 1] - 2*q[3, 3], 2*q[2, 3] + 2*q[0, 1]] , [2*q[1, 3] + 2*q[0, 2] , 2*q[2, 3] - 2*q[0, 1] , 1.0 - 2*q[1, 1] - 2*q[2, 2]]] ) return rot_matrix ''' ************************************************************ 功能：旋转矩阵 转 四元数 输入：3×3的旋转矩阵 返回值：1×4的四元数向量 ************************************************************ ''' def R2q(R): q=np.array([0.0,0.0,0.0,0.0]) q[0]=(math.sqrt(abs(np.trace(R))+1))/2.0 q[1]=(R[1,2]-R[2,1])/(4*q[0]) q[2]=(R[2,0]-R[0,2])/(4*q[0]) q[3]=(R[0,1]-R[1,0])/(4*q[0]) return q ''' ************************************************************ 功能：旋转矩阵 转 变换矩阵 输入：3×3的旋转矩阵 三维平移向量 返回值：4×4的变换矩阵 ************************************************************ ''' def R2T(R,t): rot_t = t.reshape(3,1) tran_matix = np.concatenate((R,rot_t),axis=1) matix_0 =np.array([0,0,0,0]).reshape(1,4) tran_matix = np.concatenate((tran_matix,matix_0),axis=0) return tran_matix ''' ************************************************************ 功能：变换矩阵 转 旋转矩阵 输入：4×4的变换矩阵 返回值：3×3的旋转矩阵 ************************************************************ ''' def T2R(T): rot_matix = T[0:3,0:3] return rot_matix ''' ************************************************************ 功能：主函数 1.计算任一像素在相机坐标系与世界坐标系下的坐标 2.将图像拼接成点云 ************************************************************ ''' if __name__ == "__main__": # 1.利用OpenCV读取RGB图和深度图 img = [] img.append(cv2.imread('/home/zk/test/depth/1.pgm',-1)) img.append(cv2.imread('/home/zk/test/depth/2.pgm',-1)) img.append(cv2.imread('/home/zk/test/depth/3.pgm',-1)) img.append(cv2.imread('/home/zk/test/depth/4.pgm',-1)) img.append(cv2.imread('/home/zk/test/depth/5.pgm',-1)) img_shape = np.shape(img[0]) rgb = [] rgb.append(cv2.imread('/home/zk/test/color/1.png',-1)) rgb.append(cv2.imread('/home/zk/test/color/2.png',-1)) rgb.append(cv2.imread('/home/zk/test/color/3.png',-1)) rgb.append(cv2.imread('/home/zk/test/color/4.png',-1)) rgb.append(cv2.imread('/home/zk/test/color/5.png',-1)) # 2.使用之前所完成的函数,将所给的外参转换成变换矩阵 data = np.zeros((5,7),dtype=float) f = open("/home/zk/test/pose.txt") lines = f.readlines() data_row = 0 for line in lines: list = line.strip('\n').split(' ') data[data_row:] = list[0:7] data_row+=1 tran_matix = [] tran_matix.append( R2T(q2R([data[0,6],data[0,3],data[0,4],data[0,5]]),data[0,0:3])) tran_matix.append( R2T(q2R([data[1,6],data[1,3],data[1,4],data[1,5]]),data[1,0:3])) tran_matix.append( R2T(q2R([data[2,6],data[2,3],data[2,4],data[2,5]]),data[2,0:3])) tran_matix.append( R2T(q2R([data[3,6],data[3,3],data[3,4],data[3,5]]),data[3,0:3])) tran_matix.append( R2T(q2R([data[4,6],data[4,3],data[4,4],data[4,5]]),data[4,0:3])) # 3.利用所给的相机内参和图像深度信息,将所有像素坐标转换成相机坐标 # 4.利用步骤2计算出的变换矩阵,将所有相机坐标转换成世界坐标 in_matix = np.array([[518.0,0 ,325.5], [0 ,519.0,253.5], [0 ,0 ,1 ]]) inverse_in_matix = np.linalg.inv(in_matix) num = 0 CCS_matix = [] world_matix = [] PointCloud = [] PointClouds = [] for p in range(5): print(p) for i in range(480): for j in range(640): CCS_matix.append( np.array(np.dot(inverse_in_matix,np.array([j,i,1]))*(img[p][i][j]/1000.0)) ) #求出相机坐标 world_matix.append( np.dot(tran_matix[p],[CCS_matix[num][0],CCS_matix[num][1],CCS_matix[num][2],1]) ) PointCloud.append( np.array(([world_matix[num][0],world_matix[num][1],world_matix[num][2],rgb[p][i][j][0],rgb[p][i][j][1],rgb[p][i][j][2]]), dtype = np.float32) ) num = num + 1 PointCloud = np.array(PointCloud).flatten() # ROS相关操作，将点云信息发布出去 #建立节点并建立发布者 rospy.init_node('test', anonymous=True) pub_cloud = rospy.Publisher("/points", PointCloud2,queue_size=10) #创建点云并将矩阵赋值给点云 while not rospy.is_shutdown(): pcloud = PointCloud2() #设置点云格式 #宽与高 pcloud.height = 1 pcloud.width = img_shape[0]*img_shape[1]*5 #二进制数据通道 pcloud.fields = [PointField('x',0,PointField.FLOAT32,1), PointField('y',4,PointField.FLOAT32,1), PointField('z',8,PointField.FLOAT32,1), PointField('b',12,PointField.FLOAT32,1), PointField('g',16,PointField.FLOAT32,1), PointField('r',20,PointField.FLOAT32,1)] #小端格式 pcloud.is_bigendian = False #点的长度 pcloud.point_step = 24 #行的长度 pcloud.row_step = pcloud.point_step*pcloud.width #实际数据（二进制的数据流） pcloud.data = PointCloud.tostring() #如果包含Inf / NaN值，则为false，否则为真 pcloud.is_dense = True pcloud.header.frame_id = "/map" pub_cloud.publish(pcloud) rospy.sleep(1.0)
我遇到的各种坑：
1.ROS系统的教程国内的几乎都是用C++编写的，所以网上几乎找不到python的ROS编程教程，可能是因为ROS的相关教程都是在好几年前编写的，
那时的python还不想现在这么的火，所以只要去读英文文档，但是我觉得英文文档太简洁了，对于新手太不友好了。
2.自己的很多基础知识学得不牢靠，比如python的数组默认为是64位的，以及如何定义数据流，数据流是怎样输入的，都是自己的拦路虎。
3.自己开始还不知道头的定义是怎么回事,重定义了...
关于本次操作的介绍：
1.定义节点，并通过节点发送数据
2.对于数据的格式进行定义，并且把数据给传输进去
3.catkin编译
4.运行文件
效果图:
