kitti之ros可视化_学习笔记--第4课：车子模型的添加【转载】_ros visualization_msgs::marker自定义小车的外观。

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

   
   

   
   
1
1

ylh@y:~/all_ws$ catkin_make

   
   

   
   
1
1

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

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)

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14

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

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

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<span class="token operator">=</span>Marker<span class="token punctuation">(</span><span class="token punctuation">)</span>
mesh_marker<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id<span class="token operator">=</span>FRAME_ID
mesh_marker<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp<span class="token operator">=</span>rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span>

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

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

<span class="token comment">#设计车子模型的旋转量</span>
q<span class="token operator">=</span>tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span>np<span class="token punctuation">.</span>pi<span class="token operator">/</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token comment">#(np.pi/2,0,np.pi)这里根据下载的车子模型放置位置进行调整</span>
mesh_marker<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>x<span class="token operator">=</span>q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token comment">#这里是一些模型四元数参数赋值;四元数简单理解就是表示方位这么一个量，详细可google维基百科</span>
mesh_marker<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>y<span class="token operator">=</span>q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
mesh_marker<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>z<span class="token operator">=</span>q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
mesh_marker<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>w<span class="token operator">=</span>q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

<span class="token comment">#设置车子模型的颜色</span>
mesh_marker<span class="token punctuation">.</span>color<span class="token punctuation">.</span>r<span class="token operator">=</span><span class="token number">1.0</span>
mesh_marker<span class="token punctuation">.</span>color<span class="token punctuation">.</span>g<span class="token operator">=</span><span class="token number">1.0</span>
mesh_marker<span class="token punctuation">.</span>color<span class="token punctuation">.</span>b<span class="token operator">=</span><span class="token number">1.0</span>
mesh_marker<span class="token punctuation">.</span>color<span class="token punctuation">.</span>a<span class="token operator">=</span><span class="token number">1.0</span>

<span class="token comment">#设置车子模型的大小</span>
mesh_marker<span class="token punctuation">.</span>scale<span class="token punctuation">.</span>x<span class="token operator">=</span><span class="token number">0.6</span>
mesh_marker<span class="token punctuation">.</span>scale<span class="token punctuation">.</span>y<span class="token operator">=</span><span class="token number">0.6</span>
mesh_marker<span class="token punctuation">.</span>scale<span class="token punctuation">.</span>z<span class="token operator">=</span><span class="token number">0.6</span>

model_pub<span class="token punctuation">.</span>publish<span class="token punctuation">(</span>mesh_marker<span class="token punctuation">)</span><span class="token comment">#把模型发布出去</span>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62

对于执行文件，p5_kitti.py：

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

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<span class="token operator">=</span>rospy<span class="token punctuation">.</span>Rate<span class="token punctuation">(</span><span class="token number">10</span><span class="token punctuation">)</span>
<span class="token keyword">while</span> <span class="token operator">not</span> rospy<span class="token punctuation">.</span>is_shutdown<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token comment">#读取图片</span>
    image<span class="token operator">=</span>read_camera<span class="token punctuation">(</span>os<span class="token punctuation">.</span>path<span class="token punctuation">.</span>join<span class="token punctuation">(</span>DATA_PATH<span class="token punctuation">,</span><span class="token string">'image_02/data/%010d.png'</span><span class="token operator">%</span>frame<span class="token punctuation">)</span><span class="token punctuation">)</span>
    
    <span class="token comment">#发布图片</span>
    publish_camera<span class="token punctuation">(</span>cam_pub<span class="token punctuation">,</span>bridge<span class="token punctuation">,</span>image<span class="token punctuation">)</span>        
    
    <span class="token comment">#读取点云</span>
    point_clond<span class="token operator">=</span>read_point_cloud<span class="token punctuation">(</span>os<span class="token punctuation">.</span>path<span class="token punctuation">.</span>join<span class="token punctuation">(</span>DATA_PATH<span class="token punctuation">,</span><span class="token string">'velodyne_points/data/%010d.bin'</span><span class="token operator">%</span>frame<span class="token punctuation">)</span><span class="token punctuation">)</span>

    <span class="token comment">#发布点云</span>
    publish_point_cloud<span class="token punctuation">(</span>pcl_pub<span class="token punctuation">,</span>point_clond<span class="token punctuation">)</span>

    <span class="token comment">#发布车子模型marker;由于不需要读取资料，所以直接发布即可</span>
    publish_car_model<span class="token punctuation">(</span>model_pub<span class="token punctuation">)</span>
    
    <span class="token comment">#发布</span>
    rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"published"</span><span class="token punctuation">)</span>
    rate<span class="token punctuation">.</span>sleep<span class="token punctuation">(</span><span class="token punctuation">)</span>
    frame<span class="token operator">+=</span><span class="token number">1</span>
    frame<span class="token operator">%=</span><span class="token number">154</span>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39

2、运行操作注意点

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

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

 
 

 
 
1
1

注意，data_utils.py，publish_utils.py文件也是需要执行这个操作的。

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

3）.py文件编写注意：

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

 
 

 
 
1
2
1
2

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

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

ylh@y:~$ roscore 

 
 

 
 
1
1

效果样式：

SUMMARY
========
1
2

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]

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16

终端2：rosrun [packagename] xxx.py

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

 
 

 
 
1
1

效果样式：

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

 
 

 
 
1
2
3
1
2
3

终端3：rviz

rviz

 
 

 
 
1
1

效果样式：

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).

 
 

 
 
1
2
3
4
5
6
1
2
3
4
5
6

3、效果

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

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

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

4、个人碰到一些问题

1）车子模型dae文件存在问题，我这里显示只有一半而已：

但是，影响不大，哈哈哈，白嫖东西，能用就行，不要要求太高。

2）一开始加载的模型方位不对，如图：

通过调整publish_utils.py文件publish_car_model函数中，

#设计车子模型的旋转量
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]

 
 

 
 
1
2
3
4
5
6
1
2
3
4
5
6

此外，车子大小也是可以修改函数中对应部分就可以了。publish_car_model该函数定义颜色属性也是不起效果的。

调整过后，就可以得到上面3中效果图。

至此，利用marker，读取车子dae文件，建立publisher并发布到ros，在rviz可视化车子模型的任务完成了～
期待下一节课，添加2号相机视野指示

我遇到的问题
在我运行一样的代码是发生了报错：
bobo@bobo-virtual-machine:~/kitti_cakin_ws/src/kitti_tutorial/src$ rosrun kitti_tutorial kitti.py
from: can’t read /var/mail/data_utils
from: can’t read /var/mail/publish_utils
/home/bobo/kitti_cakin_ws/src/kitti_tutorial/src/kitti.py: 行 5: DATA_PATH: 未找到命令
/home/bobo/kitti_cakin_ws/src/kitti_tutorial/src/kitti.py: 行 9: 未预期的符号 'kitti_node',' 附近有语法错误 /home/bobo/kitti_cakin_ws/src/kitti_tutorial/src/kitti.py: 行 9: rospy.init_node(‘kitti_node’, anonymous=True)’
但是经过检查，代码是没有问题的；
排除过后发现是因为我在第一行导入python解释器的时候将代码格式化了一下，Python脚本中，通常使用#!/usr/bin/env python来指定解释器路径，而您使用了# !/usr/bin/env python，这两者之间存在空格，可能导致解释器无法正确识别。
删掉空格之后可以正常运行了。

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

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