自动驾驶数据集KITTI【3】发布点云资料

1.前言

在代码 kitti.py 中新建一个发布点云的publisher。然后读取资料，并发布出去。

ROS中有一种格式叫 PointCloud2 专门用来发布点云。
每个bin文件里面都是一些点，每个点有4个数据。
把数据通过numpy读入：

point_cloud = numpy.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame), dtype = numpy.float32).reshape(-1,4)

下一步，将点云用PointCloud2的形式发布出去。需要使用point_cloud2的资料库。由ros提供，可以import进来。

import sensor_msgs.point_cloud2 as pcl2

pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_cloud[:,:3])) #取前三个数据，不用反射率

pcl2.create_cloud_xyz32函数表示xyz坐标都是float32格式数据。会把这个矩阵转换成PointCloud2的形式。
在使用该函数之前，需要通过Header添加数据的信息，包括时间和frame名称，名称可以自定义。

from std_msgs.msg import Header

header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'

2.本节所使用到的代码：

#!/usr/bin/env python3
import rospy
import os
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image,PointCloud2

#发布点云数据
import numpy as np
from std_msgs.msg import Header
import sensor_msgs.point_cloud2 as pcl2




DATA_PATH = "/home/sk/Desktop/data/kitti/2011_09_26/2011_09_26_drive_0005_sync/"    

if __name__ == "__main__":
    frame =0
    rospy.init_node("kitti_node",anonymous=True)
    cam_pub  = rospy.Publisher("kitti_cam", Image, queue_size=10)
    pcl_pub  = rospy.Publisher("kitti_point_cloud", PointCloud2, queue_size=10)

    #发布点云数据
    bridge = CvBridge()
    
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        img = cv2.imread(os.path.join(DATA_PATH, "image_02/data/%010d.png"%frame))
        point_cloud = np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame),dtype=np.float32).reshape(-1,4)
        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = 'map'
        pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_cloud[:,:3]))
        cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))
        rospy.loginfo("kitti published")
        rate.sleep()
        frame +=1
        frame %=154

运行代码之后，通过rviz可视化数据，具体实现为add/by topic/point_cloud