【从kitti开始自动驾驶】--2.1 发布照片+点云

“长路落满了光”

• 1. 预知识
• 2. python文件新增代码详解
• 3. 执行,查看效果
• 4. python文件源码

本节将在上一节的基础上,进行功能的添加,其他内容不变, 参考.

数据集放置在kitti_folder/2011_09_26/文件夹下,结构如下:

本节用到的是点云数据:kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/velodyne_points/data/下的数据,共有153份
工作空间是test3_autodrive_ws
python代码存在test3_autodrive_ws/src/demo1_kitti_pub_photo/scripts/kitti.py

1. 预知识

从文件中可以看到,点云数据全是二进制文件,每个点包含四个数据,分别是:(x axis, y axis, z axis, 折射率)
因为是激光雷达扫描到的数据,所以包含三个坐标和折射四个参数,如下:
这种点云成为point_cloud或者简称PCL,以后的库会看到.

2. python文件新增代码详解

from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from std_msgs.msg import Header

• 引进点云数据类型的消息头文件
• Header 是为了发布点云消息的header信息

---

pcl_pub = rospy.Publisher("kitti_point_cloud", PointCloud2, queue_size=10)

• 创建点云消息的发布者

---

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

• 读取数据,dtype=np.float32告诉numpy这是浮点数据
• 读取之后,是一个一维的阵列,reshape之后,转换成nX4的矩阵,n就是点的数量,4就是x,y,z和折射率

---

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

• 这是发布点云消息的header(描述这一簇点云的信息)
• 包含时间和坐标系(可以随意改)

---

pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))

• 从point_cloud中读取数据并且进行转换发布
• pcl2只支持3维,所以[:, :3]第一个冒号代表全部的点,第二个冒号代表取前三个数据,丢掉折射率
• 转换过程:NX4的矩阵->NX3的矩阵->pcl2的数据形式

---

rospy.loginfo('camera image and point cloud published')

• 再修改一下info

---

3. 执行,查看效果

1. 首先启动roscore ;
2. 然后运行:

rosrun demo1_kitti_pub_photo kitti.py 

3. 打开rviz,add by topic,看到kitti_cam下的image和point_cloud,勾选,可以看到如下效果(视频形式):

4. python文件源码

#!/usr/bin/env python
#coding:utf-8
import os
import cv2
import rospy
import numpy as np
from sensor_msgs.msg import Image, PointCloud2
from std_msgs.msg import Header
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge


DATA_PATH = '/home/qinsir/kitti_folder/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)

    brige = CvBridge()      #opencv支持的图片和ROS可以读取的图片之间转换的桥梁

    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        #使用OS,路径串接,%010d,这个字串有10个数字(比如0000000001).png
        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)
        
        
        cam_pub.publish(brige.cv2_to_imgmsg(img, "bgr8"))   #opencv -> ROS 格式 蓝 绿 红 8位(0-255)

        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = "map"
        pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))
        rospy.loginfo('camera image and point cloud published')
        rate.sleep()
        frame += 1
        frame %= 154