kitti之ros可视化_学习笔记--第3课：点云资料发布

Posted 2021-05-16 鸿_H

环境：ubuntu16.04，ros-kinetic，python2，vscode，opencv，rviz

概要：这节课笔记，展示的是，发表kitti数据集中的点云资料到ros里面，并在rviz显示。

资料准备及预处理可参考博客，https://blog.csdn.net/qq_45701501/article/details/116447770。

1、源码及解析

首先，使用catkin_create_pkg [packageName] [依赖]，创建一个包，如

ylh@y:~/all_ws/src$ catkin_create_pkg kitti_tutorial_compare rospy

然后，返回工作空间上一级目录，使用catkin_make编译工作空间，如

ylh@y:~/all_ws$ catkin_make

最后，才把源码放到该包的src文件夹中。
如果还有关于包创建不了解的，可以百度一下或者查看个人之前的博客。

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import cv2#导入图片处理包
import os#导入路径处理包 
import numpy as np#更改numpy名称为np

import rospy
from std_msgs.msg import Header#ros提供的包，从而建立Header
from sensor_msgs.msg import Image,PointCloud2
from cv_bridge import CvBridge #cvbridge包是使得图片可以在cv以及ros之间相互转换
import sensor_msgs.point_cloud2 as pcl2#将这个传感器点云数据更改名称为pcl2

DATA_PATH='/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync'#读取图片路径，根据自己kitti数据集保存路径进行填写

if __name__=='__main__':
    frame=0#目标是发布连续图片
    rospy.init_node('kitti_node',anonymous=True) #创建一个node
    cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)#创建一个发布图片publisher
    pcl_pub=rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#创建一个发布点云的publisher
    bridge=CvBridge()#创建ros和cv的图片相互转换的桥梁

    rate=rospy.Rate(10)#每秒10次的频率把资料发送出去
    while not rospy.is_shutdown():
        #img=cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%0))#读取具体文件夹image_02相机中的具体图片，也就是发布该图片到topic
        img=cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))#发布连续图片
        #bridge.cv2_to_imgmsg(img,"bgr8")#具体的装换使用，将cv的图片格式，转换为ros可识别的图片格式
        
        #numpy是一个处理数据的python模块，语句前半部分读取路径上的点云数据,数据还属于一维阵列，reshape（-1,4）就是将数据转换为nx4的矩阵
        point_clond=np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame),dtype=np.float32).reshape(-1,4)
        
        cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))#bgr8描述图片的属性

        header=Header()
        header.stamp=rospy.Time.now()
        header.frame_id='map'#frame_id表示坐标系名称，‘map’可以任意取
        #create_cloud_xyz32将读取的点云数据的xyz转换为ros里面的PointCloud2形式,但是
        #这里的只是xyz坐标值，不包含反射率，[:,:3]中第一个：表示读取全部的4个，然后：3取前面三个，
        #那么，经过该函数的处理就会变成nx3的矩阵
        pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_clond[:,:3]))

        #rospy.loginfo("camera image published")
        rospy.loginfo("published")
        rate.sleep()
        frame+=1#迭代读取下一张图片
        frame%=154#由于文件夹中最多只有153图片，超出了，又会回到0,循环

这里一定要注意，资料读取路径的填写。个人kitti资料预处理可参考博客https://blog.csdn.net/qq_45701501/article/details/116447770。

2、运行操作注意点

1）终端编译.py文件指令：chmod +x xxx.py，如：

ylh@y:~/all_ws/src/kitti_tutorial_compare/src$ chmod +x p4_kitti.py 

2）发布的三步，读取kitti对应数据，建立发布函数，发布。

3）.py文件编写注意：

#!/usr/bin/env python
# -*- coding: UTF-8 -*-

这两句话，一个是表示是python文件，一个是避免中文注释带来bug，并且这俩一定要放到文件开头，我是python小白，这是个人碰壁得到的经验，内在原因不清楚，如果是python熟悉的小伙伴，可以给点建议哈。

4）运行时，依次执行：
终端1：roscore

ylh@y:~$ roscore 

效果样式：

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.17

NODES

auto-starting new master
process[master]: started with pid [29097]
ROS_MASTER_URI=http://y:11311/

setting /run_id to 6a5ead02-aed2-11eb-997f-d45d64d7a8c2
process[rosout-1]: started with pid [29111]
started core service [/rosout]

终端2：rosrun [packagename] xxx.py

ylh@y:~/all_ws/src/kitti_tutorial_compare/src$ rosrun kitti_tutorial_compare p4_kitti.py 

效果样式：

[INFO] [1620350984.000907]: camera image published
[INFO] [1620350984.103306]: camera image published
[INFO] [1620350984.200628]: camera image published

终端3：rviz

rviz

效果样式：

ylh@y:~$ rviz
[ INFO] [1620350716.287772508]: rviz version 1.12.17
[ INFO] [1620350716.287799198]: compiled against Qt version 5.5.1
[ INFO] [1620350716.287805118]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1620350716.901989725]: Stereo is NOT SUPPORTED
[ INFO] [1620350716.902087295]: OpenGl version: 4.5 (GLSL 4.5).

3、效果

选择topic，让图片在rviz中显示：Add-By topic-[topicname]，如图

如果想下次不用再依次选择加载topic的话，可以选择rviz-save config as-xxx.rviz来保存现有配置，下次就可以直接选择打开xxx.rviz，若增加了topic，再次保存配置即可。

显示效果：
这里，我们发现点云显示效果有点细小。通过调整size参数，可以增大。这里，我把size参数由0.01调整为0.05，最后效果如图

至此，读取kitti数据集中点云数据，建立publisher并发布到ros，在rviz可视化的任务完成了～
期待下一节课，添加车子模型

#####################
学习课程来源up主，AI葵：
https://www.youtube.com/watch?v=TBdcwwr5Wyk

致谢AI葵老师
不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈