天天看點
傳回

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] [依賴]

，建立一個包，如

[email protected]:~/all_ws/src$ catkin_create_pkg kitti_tutorial_compare rospy

然後，傳回工作空間上一級目錄，使用

catkin_make

編譯工作空間，如

[email protected]:~/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

，如：

[email protected]:~/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

 
[email protected]:~$ 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

 
[email protected]:~/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

效果樣式：

[email protected]:~$ 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]

，如圖

kitti之ros可視化_學習筆記--第5課：相機視野訓示線添加

如果想下次不用再依次選擇加載topic的話，可以選擇

rviz-save config as-xxx.rviz

來儲存現有配置，下次就可以直接選擇打開

xxx.rviz

，若增加了topic，再次儲存配置即可。

最後，結合前面課程，效果可達到如圖：

kitti之ros可視化_學習筆記--第5課：相機視野訓示線添加

當添加新增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葵老師

不積矽步，無以至千裡

好記性不如爛筆頭

感覺有點收獲的話，麻煩大大們點贊收藏哈

KITTI
上一篇: 【轉】iOS websocket 及時通訊實作
下一篇: 安卓學習：碎片切換

繼續閱讀