ROS可视化KITTI数据集

KITTI数据集是世界范围内影响力最广的自动驾驶数据集之一，由四个光学摄像头数据集、一个Velodyne 64线3D激光雷达数据集和标定文件等组成。

其中图像文件以png格式储存，点云数据以bin格式储存。

利用ROS系统下的RViz工具来进行图像和点云的同步可视化。

主要步骤如下：

1. 创建一个catkin程序包

   cd ~/catkin_ws/src  #切换到ros工作空间
   catkin_create_pkg kitti_tutorial rospy  #创建新的package
   cd ..
   catkin_make

2. 读取数据并进行发布
   重新整理一下ros发布话题的步骤吧
   ①首先初始化一个ros节点，这是ros通信的基础
   ②创建发布者，用于读取数据并将其以ros消息的形式发布出去
   ③设置发布速率
   ④发布消息
   这里我分别创建了data_utils.py和publish_utils.py工具包以简化主程序run.py
   将三个文件放在kitti_tutorial/src目录下

   """data_utils.py,用于协助读入图像和点云数据"""
   import cv2
   import numpy as np
   
   def read_camera(path):
    return cv2.imread(path)
   
   def read_point_cloud(path):
    return np.fromfile(path, dtype = np.float32).reshape(-1, 4)

   """publish_utils.py,用于协助ros进行消息的发布"""
   import rospy
   from visualization_msgs.msg import Marker
   from geometry_msgs.msg import Point
   from sensor_msgs.msg import Image, PointCloud2
   from cv_bridge import CvBridge
   from std_msgs.msg import Header
   import sensor_msgs.point_cloud2 as pcl2
   
   FRAME_ID = 'map' #坐标系名称
   
   def publish_camera(cam_publisher, bridge, image):
    cam_publisher.publish(bridge.cv2_to_imgmsg(image, 'bgr8')) #用于把opencv读入的图片转化为ros可以读取的格式
    
   def publish_point_cloud(pc_publisher, point_cloud):
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = FRAME_ID  #锚定坐标轴
    pc_publisher.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))  #这里只发布了点云的xyz坐标
   
   def publish_ego_car(ego_car_publisher):
    '''在点云坐标系下发布45度的摄像机视野线'''
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()
    
    marker.id = 0 #标志的唯一ID
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration() #生命周期
    marker.type = Marker.LINE_STRIP
    
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 1.0  #透明度
    marker.scale.x = 0.2  #线条宽度
    
    marker.points = []
    marker.points.append(Point(10, -10, 0))
    marker.points.append(Point(0, 0, 0))
    marker.points.append(Point(10, 10, 0))
    
    ego_car_publisher.publish(marker)

   """run.py，用于最终执行"""
   #!/usr/bin/env python #Shebang魔法指令
   import os
   import rospy
   from cv_bridge import CvBridge
   from visualization_msgs.msg import Marker
   from sensor_msgs.msg import Image, PointCloud2
   from data_utils import read_camera, read_point_cloud
   from publish_utils import publish_camera, publish_point_cloud, publish_ego_car
   
   DATA_PATH = '/home/zhangyuqin/Projects/KITTI/RowData/2011_09_26_drive_0070_sync/'
   
   if __name__ == '__main__':
    rospy.init_node('kitti_node', anonymous = True)  #初始化节点
    cam_publisher = rospy.Publisher('kitti_cam_publisher', Image, queue_size = 10)  #创建摄像头publiser
    pc_publisher = rospy.Publisher('kitti_pc_publisher', PointCloud2, queue_size = 10)  #创建激光雷达publiser
    ego_publisher = rospy.Publisher('kitti_ego_car', Marker, queue_size = 10)  #创建车辆模型publisher
    
    bridge = CvBridge()
    rate = rospy.Rate(5)
    frame = 0  #标记初始化帧数
    
    while not rospy.is_shutdown():
        img = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))  #读入图片消息数据。格式化输出，%010d表示10位整数
        pc = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))  #读入点云消息数据
        publish_camera(cam_publisher, bridge, img)  #发布
        publish_point_cloud(pc_publisher, pc)
        publish_ego_car(ego_publisher)
        rospy.loginfo("camera image published")
        rate.sleep()
        frame += 1
        frame %= 420  #这个数据集一共420帧，其实可以多写几行来自动读取帧数，懒了
   

   编写好以上三个源文件后，还需要赋予run.py可执行权限，rosrun运行我们创建的节点

   roscd kitti_tutorial/src  #roscd一步到位，切换到ros工作空间
   chmod +x run.py  #赋予文件可执行权限
   rosrun kitti_tutorial run.py

3. 启动图形化模拟环境rviz，接受消息并可视化
   打开rviz可视化工具

   rviz

   打开后是空界面，点左下角add，可以看到我们刚刚发布的话题，从中选择我们需要的消息并读入
   
   可视化完成