本节将在上一节的基础上,进行功能的添加,其他内容不变, 参考.
数据集放置在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. 执行,查看效果
- 首先启动roscore ;
- 然后运行:
rosrun demo1_kitti_pub_photo kitti.py
- 打开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