kitti之ros可视化_学习笔记--第5课：相机视野指示线添加

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

概要：这节课笔记，新增展示的是，使用marker，将相机视野范围指示线发布到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文件夹中。
如果还有关于包创建不了解的，可以百度一下或者查看个人之前的博客。

这里为了便于文件的编写，我们把读取kitti数据文件，发布函数定义文件，执行文件分成三个文件，分别对应data_utils.py，publish_utils.py，p5_kitti.py。

发布相机指示线使用到的marker知识，可参考链接http://wiki.ros.org/rviz/DisplayTypes/Marker官方文档介绍。

对于读取数据文件，data_utils.py：

#!/usr/bin/env python
# -*- coding:utf8 -*-

import cv2
import numpy as np
import os

#读取图片路径函数
def read_camera(path):
    return cv2.imread(path)

#读取点云路径函数
def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshape(-1,4)

对于发布函数定义文件，publish_utils.py：

#!/usr/bin/env python
# -*- coding:utf8 -*-

import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker#绘制相机视野指示线模块
from sensor_msgs.msg import Image,PointCloud2
from geometry_msgs.msg import Point#Point来自ros包定义，所以需要定义;若不清楚，则需要到ros官网上面查看具体那个包
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import numpy as np
import tf

FRAME_ID='map'

#发布图片函数
def publish_camera(cam_pub,bridge,image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))

#发布点云函数
def publish_point_cloud(pcl_pub,point_clond):
    header=Header()
    header.stamp=rospy.Time.now()
    header.frame_id=FRAME_ID
    pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_clond[:,:3]))

#发布车子外形函数
def publish_car_model(model_pub):
    
    mesh_marker=Marker()
    mesh_marker.header.frame_id=FRAME_ID
    mesh_marker.header.stamp=rospy.Time.now()

    mesh_marker.id=-1#id只能设置整数，不能设置带有小数的
    mesh_marker.lifetime=rospy.Duration()
    mesh_marker.type=Marker.MESH_RESOURCE#这里的MESH_RESOURCE表示导入的是3d模型
    mesh_marker.mesh_resource="package://kitti_tutorial/Audi R8/Models/Audi R8.dae"#下载的dae模型存在问题，只是显示部分

    #设定模型位置
    mesh_marker.pose.position.x=0.0
    mesh_marker.pose.position.y=0.0
    mesh_marker.pose.position.z=-1.73#这里负数，是因为以激光雷达坐标系而定义的，1.73是根据官方发布的位置定义所取的

    #设计车子模型的旋转量
    q=tf.transformations.quaternion_from_euler(0,0,np.pi/2)#(np.pi/2,0,np.pi)这里根据下载的车子模型进行调整
    mesh_marker.pose.orientation.x=q[0]
    mesh_marker.pose.orientation.y=q[1]
    mesh_marker.pose.orientation.z=q[2]
    mesh_marker.pose.orientation.w=q[3]

    #设置车子模型的颜色
    mesh_marker.color.r=1.0
    mesh_marker.color.g=1.0
    mesh_marker.color.b=1.0
    mesh_marker.color.a=1.0

    #设置车子模型的大小
    mesh_marker.scale.x=0.6
    mesh_marker.scale.y=0.6
    mesh_marker.scale.z=0.6

    model_pub.publish(mesh_marker)#把模型发布出去

#相机视野函数
def publish_ego_car(ego_car_pub):
#publish left and right 45 degree FOV lines and ego car model mesh
    
    #marker_array=MarkerArray()

    marker=Marker()
    marker.header.frame_id=FRAME_ID
    marker.header.stamp=rospy.Time.now()

    marker.id=0#每个marker只能有一个id，有重复的id，只会显示一个
    marker.action=Marker.ADD#表示添加marker
    marker.lifetime=rospy.Duration()#lifetime表示marker在画面中显示的时长;Duration()函数，不给任何参数时，表示一直存在
    marker.type=Marker.LINE_STRIP#所发布marker的类型

    #设定指示线颜色
    marker.color.r=0.0
    marker.color.g=1.0
    marker.color.b=0.0
    marker.color.a=1.0#透明度，1表示完全不透明
    marker.scale.x=0.2#大小，这里表示线的粗细

    #根据激光点云的坐标系来定义2号相机的视野范围
    #根据官方材料俯视图中2号相机，其的视野范围为90度，以激光雷达作为坐标系的话，左右各取45度
    #这里为了展示视野范围，根据45度几何关系取三个点（x,y），也就是（10,-10），（0,0），（10,10），三点连接就可得到相机视野
    marker.points=[]
    marker.points.append(Point(10,-10,0))#Point,属于ros的资料包里面的定义，所以需要导入
    marker.points.append(Point(0,0,0))
    marker.points.append(Point(10,10,0))

    ego_car_pub.publish(marker)

对于执行文件，p5_kitti.py：

#!/usr/bin/env python
# -*- coding:utf8 -*-

from data_utils import *
from publish_utils import *

DATA_PATH='/home/ylh/data/kitti/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)#建立发布图片topic
    pcl_pub=rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#建立发布点云topic
    ego_pub=rospy.Publisher('kitti_ego_car',Marker,queue_size=10)#建立发布指示线marker的topic
    model_pub=rospy.Publisher('kitti_car_model',Marker,queue_size=10)#建立发布车子模型的marker的topic
    bridge=CvBridge()

    rate=rospy.Rate(10)
    while not rospy.is_shutdown():
        #读取图片
        image=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        
        #发布图片
        publish_camera(cam_pub,bridge,image)        
        
        #读取点云
        point_clond=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))

        #发布点云
        publish_point_cloud(pcl_pub,point_clond)

        #发布相机视野指示线marker;由于不需要读取资料，所以直接发布即可
        publish_ego_car(ego_pub)

        #发布车子模型marker;由于不需要读取资料，所以直接发布即可
        publish_car_model(model_pub)
        
        #发布
        rospy.loginfo("published")
        rate.sleep()
        frame+=1
        frame%=154

2、运行操作注意点

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

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

注意，data_utils.py，publish_utils.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，再次保存配置即可。

最后，结合前面课程，效果可达到如图：

当添加新增topic后，发现有两条指示线出现在rviz里面时候，表示相机视野指示线添加成功。

4、个人认为关键点

1）重点理解一切坐标表示，需要转换到激光雷达坐标系进行表示

2）2号相机是朝向正前方，成角90度，并被激光雷达坐标系的x轴平分;其他相机视野指示线定义也是如此，先获取视野范围，选取特定点，再用marker表示即可。

3）marker知识，http://wiki.ros.org/rviz/DisplayTypes/Marker

至此，利用marker，建立相机视野范围指示线，以2号相机为例，建立publisher并发布到ros，在rviz可视化相机视野范围的任务完成～

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

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