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

【ROS】发布点云资料

把kitti中的点云资料发布到点云空间中。
在代码 kitti.py 中新建一个发布点云的publisher。然后读取资料，并发布出去。
ROS中有一种格式叫 PointCloud2 专门用来发布点云。

点云数据位于 velodyne文件夹中。

每个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'

完整代码：

#!/usr/bin/python
import cv2
import os

import numpy
import rospy

from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge


DATA_PATH = '/home/ros/Documents/ros-kitticlass/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)  #PointCloud2
    bridge = CvBridge()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
#        img = cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%0))
        img = cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        point_cloud = numpy.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame), dtype = numpy.float32).reshape(-1,4)
        cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))

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

        pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_cloud[:,:3]))
        rospy.loginfo("published")
        rate.sleep()
        frame+=1
        frame%=154

通过rosrun命令运行代码：

rosrun kitti_tutorial kitti.py

点击Add，添加PointCloud2

显示点云效果：