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