ROS加载kitti数据集数据，并将图像和点云资料在rviz显示出来

实现效果：

图一

1.准备数据

kitti数据集：

图二（图像资源）

图三（点云资源）

 2.发布话题

创建工作空间与功能包 

mkdir -p ~/catkin_ws/src

cd ~/catkin_ws/src

catkin_init_workspace

cd ..

catkin_make

cd src

catkin_create_pkg kitti_tutorial rospy std_msgs

cd .. 

catkin_make

在kitti_tutorial的src中创建python文件（kitti.py）内容如下：

#!/usr/bin/env python3
# -*- coding:utf-8 -*-

import os
import cv2
from cv_bridge import CvBridge
import rospy
import numpy as np
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2
import sensor_msgs.point_cloud2 as pcl2

DATA_PATH = "/home/yh/rawdata/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))
        cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))

        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]))
        rospy.loginfo("kitti published")
        rate.sleep()
        frame += 1
        frame %= 154

下来，在上面创建功能包与工作空间的基础上：

注意：DATA_PATH为自己的文件路径，需要自己去更改

cd ~/catkin_ws/src

roscd kitti_tutorial/src

chmod +x kitti.py

 下面开三个终端分别输入：

  roscore

  rosrun kitti_tutorial kitti.py

  rosrun rviz rviz

3.在rviz中加载话题

到这里就结束了最后结果和上面的一样。