Ha ha ha ha ha ha ha ha ha ha, yesterday or the program, because there is little Python-related articles on the development of ROS, we really are all kinds of attempts, the last with the help of big brother finally succeeded.
On the code:
# ! / Usr / bin / Python the env # Coding = UTF-. 8 '' ' ******************************** **************************** author: EnKant ******************* ***************************************** '' ' Import rospy Import OS Import numpy NP AS Import Math Import CV2 from sensor_msgs.msg Import PointCloud2, PointField from sensor_msgs Import point_cloud2 '' ' **************************************************************** ******************************* functions: rotation quaternion rotation matrix input: Quaternion vector returns 1 × 4 3 × 3 rotation matrix: value ************************************************** ********** '' ' 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] - Q * 2 [2, 2 ]]] ) return rot_matrix '' ' ********************************** ************************** functions: rotation quaternion rotation matrix Input: 3 × 3 rotation matrix Return Value: quaternion vector of 1 × 4 ******************************* **************************************************************** '' ' DEF R2q (R & lt): Q = np.array ([0.0,0.0,0.0 , 0.0 ]) Q [0] = (Math.sqrt (ABS (np.trace (R & lt)) +. 1)) / 2.0 Q [ . 1] = (R & lt [1,2] -R & lt [2,1]) / (. 4 * Q [0]) Q [ 2] = (R & lt [2,0] -R & lt [0,2]) / (. 4 * Q [0]) Q [ . 3] = (R & lt [0,1] - R & lt [1,0]) / (. 4 * Q [0]) return Q '' ' **************************************************************** ******************************* functions: rotation matrix transformation matrix transducer inputs: the three-dimensional translation vector rotation matrix of 3 × 3 Back Found: a 4 × 4 transform matrix ****************************************** ************************************************** '' ' DEFThe R2T (R & lt, T): rot_t = t.reshape (3,1 ) tran_matix = np.concatenate ((R & lt, rot_t), Axis =. 1 ) matix_0 = np.array ([0,0,0,0]). the RESHAPE (l, 4 ) tran_matix = np.concatenate ((tran_matix, matix_0), Axis = 0) return tran_matix '' ' ************************************************************ ************************************* functions: transformation matrix transpose rotation matrix input: 4 × 4 of transformation matrix return value: a 3 × 3 rotation matrix *************************************** ************************************************************ '' ' DEF the T2R (T): rot_matix = T [0: 3,0:. 3 ] return rot_matix ' '' ************************************************** ********** function: the main function 1 calculates coordinates of any pixel in the camera coordinate system and the world coordinate system 2. images into a point cloud *********** ************************************************* ' '' IF the __name__ == " __main__ " : # 1. OpenCV read using RGB and depth charts 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. Use previously completed a function to convert the reference to the outer transformation matrix Data np.zeros = ((5,7), DTYPE = a 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. using the internal reference to a camera and image depth information, converts all pixel coordinates into the camera coordinate # 4 using the transformation matrix is calculated in step 2, the camera will convert all coordinates to world coordinates 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-related operations, the point cloud information advertised # build node and the establishment of the publisher rospy.init_node ( ' the Test ' , Anonymous = True) pub_cloud = rospy.Publisher ( " / Points " , PointCloud2, the queue_size = 10 ) # Create a point cloud point cloud assigned to the matrix and the while Not rospy.is_shutdown (): pcloud = PointCloud2 () # set point cloud format # width and height 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 & lt ' , 20 is, PointField.FLOAT32,1 )] # little endian pcloud.is_bigendian = False # length point pcloud.point_step = 24 # line length pcloud.row_step = pcloud.point_step * pcloud.width # Actual data (binary data stream) pcloud.data = PointCloud.tostring () # If you include Inf / NaN values, was false, otherwise true pcloud.is_dense = true pcloud.header.frame_id = " / the Map " pub_cloud.publish ( pcloud) rospy.sleep ( 1.0 )
I met various pit:
1.ROS system tutorial domestic almost all written in C ++, so hardly find online tutorials python programming of ROS, ROS may be because the tutorials were written several years ago,
Then the python did not want to fire now so, so long as the document read English, but I think the English document too simple for the novice too unfriendly.
2. A lot of their firm enough in the basics, such as python array defaults to 64-bit, as well as how to define the data flow, data flow is how to enter, are their own stumbling block.
3. I started do not know how it is defined in the head, redefined ...
About this operation introduction:
1. Define node, and transmits the data through the node
2. definition of the format of the data, and to transfer the data into the
3.catkin compilation
4. Run the file
Renderings:
