(二)发布KITTI摄像头 激光雷达数据到ROS

一 、发布image_02 中的文件

在KITTI数据中有四个相机的文件分别是image_00, image_01, image_02, image_03,四个相机,本文使用image_02的彩色相机。
在这里插入图片描述
image_02下的data文件夹中,记录了每秒10帧共314张图片,即31.4s的分帧照片。其名称组成均为10位数字+png。
在这里插入图片描述
我们可以编写一个Publisher文件将文件夹中的image发布出去

创建新文件imagePublisher.py
使用ros发布图片需要处理花建立节点 话题等,通过opencv读取图片,再将图片使用CvBridge转换成ros中的可读取的格式,最后将图片发布出来。

发布图片代码
#!/usr/bin/python2
from rospy.timer import Rate
import cv2
import os
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

DATA_PATH = '/home/liqi/dev/catkin_ws/src/KITTI_tutorials/2011_09_26_drive_0014_sync/'

if __name__ == '__main__':
frame =0 
# 初始化一个节点名称设为kitti_node
rospy.init_node('kitti_node',anonymous=True)
# 发布一个话题 名字为kitti_cam 设置一秒钟发10次
cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=10)
# 使用CvBridge使ROS和opencv进行相互联系通讯
bridge = CvBridge()

rate = rospy.Rate(10)
while not rospy.is_shutdown():
# 设置图像PATH 通过frame读每一张图片
img=cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
# 使用CvBridge将opencv图像转为image
cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))

rospy.loginfo("Camera image published")
frame += 1
frame %= 314
rate.sleep()

将其转换成转成可执行文件
cd src
roscd KITTI_tutorials/src
chmod +x imagePublisher.py

rosrun KITTI_tutorials imagePublisher

新建Terminal 打开 rviz
在左下方点击add 在by topic中找到发布出来的kitti_cam话题即可看到发布出来的照片

在这里插入图片描述
编译如果报错如下:
报错:ImportError: dynamic module does not define module export function (PyInit_cv_bridge_boost)

如果电脑中安装的python是3.X版本,则采用的是默认Python版本(python3)进行执行的,采用Python3.X订阅ros中的图片就是会出现这样的问题,需要换回Python2.7。

#!/usr/bin/python
这行代码表示默认使用的是python3,将其修改为
#!/usr/bin/python2
这样就能换回Python2.7来使用了。
然后再进行实验,就能正常使用不报错了。问题解决。

二 、发布激光雷达velodyne_points的文件

处理点云文件,KITTI的点云文件是一堆二进制文件,其中记录了四组数字,分别是点的x y z坐标和此点的反射率,需要注意的是此处使用的是velodyne 的坐标系作为参考。

在这里插入图片描述
在程序中,使用np.fromfile进行读取bin文件,使用ros自带的库 pcl2.create_cloud_xyz32 读取float32点云 会将其转化成ros里的PointCloud2的格式,将(n,4)的数据转换成pcl2识别的(n,3)的数据 不接受反射率。

#!/usr/bin/python2
# coding=UTF-8
from rospy.timer import Rate
import cv2
import os
import rospy
import numpy as np
import sensor_msgs.point_cloud2 as pcl2
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2
from cv_bridge import CvBridge

DATA_PATH = '/home/liqi/dev/catkin_ws/src/KITTI_tutorials/2011_09_26_drive_0014_sync/'

if __name__ == '__main__':
    frame =0 
    # 初始化一个节点名称设为kitti_node
    #anonymous=True,要求每个节点都有唯一的名称,避免冲突。
    rospy.init_node('kitti_node',anonymous=True)
    # 发布一个话题 名字为kitti_cam 设置一秒钟发10次 
    cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=10)
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2 ,queue_size=10)
    # 使用CvBridge使ROS和opencv进行相互联系通讯
    bridge = CvBridge()
    #设置发布的频率,单位是每秒次数,这是每秒10次的频率发布主题
    rate = rospy.Rate(10)

    #rospy.is_shutdown()检查节点是否关闭
    while not rospy.is_shutdown():
        # 设置图像PATH 通过frame读每一张图片
        img=cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        # 定义其数据类型为float32 读取的资料为一维的阵列 通过reshape转为(n,4)的矩阵
        point_cloud = np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame),dtype= np.float32).reshape(-1,4)
        # 使用CvBridge将opencv图像转为image
        cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))

        # 建立header  读取ros现在的时间点和坐标系id
        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = 'map'

        # pcl2.create_cloud_xyz32 读取float32点云 会将其转化成ros里的PointCloud2的格式
        # 将(n,4)的数据转换成pcl2识别的(n,3)的数据 不接受反射率
        pcl_pub.publish(pcl2.create_cloud_xyz32(header , point_cloud[:,0:3]))
        rospy.loginfo(" published")
        frame += 1
        frame %= 314
        #睡眠一定持续时间,如果参数为负数,睡眠会立即返回
        rate.sleep()

在左下方点击add 在by topic中找到发布出来的kitti_point_cloud话题即可看到发布出来的照片
在这里插入图片描述

©️2020 CSDN 皮肤主题: 深蓝海洋 设计师:CSDN官方博客 返回首页