注意:
- 再学习本系列教程时,应该已经安装过ROS了并且需要有一些ROS的基本知识
ubuntu版本:20.04
ros版本:noetic
课程回顾
ROS1结合自动驾驶数据集Kitti开发教程(一)Kitti资料介绍和可视化
ROS1结合自动驾驶数据集Kitti开发教程(二)发布图片
ROS1结合自动驾驶数据集Kitti开发教程(三)发布点云数据
前言
上一节内容我们发布了点云数据,但是我们在看点云数据的时候会发现中间有一块很大的黑色区域,这块地方没有显示任何东西,我们可以放一个车的3D模型。并且根据发布的图像画出摄像头检测范围。
这章主要介绍Marker的主要用法。
1.Marker
通过visualization_msgs/Marker
或者 visualization_msgs/MarkerArray
消息,Marker
允许在Rviz
中显示各种基本形状的3D显示,效果如下所示:
2.添加照相机视野
kitti数据集提供的图片数据是通过一个90°的相机拍摄的,所以需要将两条线之间的夹角画成90度。相机的位置可以查看第一章中的设备安装图。具体程序如下所示:
def publish_cam_angle_line_fun(cam_angle_line_pub):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
# 每个显示的marker都需要不一样的id,否则会覆盖
marker.id = 0
marker.type = Marker.LINE_STRIP# 直线
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()#永久显示
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0 # 透明度
marker.scale.x = 0.2 # 线粗细
# 这边点的数据主要看官方提供的位置图
# 会在每两个连续点之间画一条线0-1,1-2。。。
marker.points = []
marker.points.append(Point(10,-10,0))
marker.points.append(Point(0,0,0))
marker.points.append(Point(10,10,0))
cam_angle_line_pub.publish(marker)
2.1效果
画完视角的效果如下所示:
3.添加自车模型
3.1下载车辆模型
根据Marker官网解释说需要使用.stl
、.mesh
或者.dae
模型。在google上输入car dae
关键词。Car dae Free3D
笔者找了很多模型,但是直接能用的很少,如果模型不能正常被rviz读取的话会非法占用内存,导致电脑卡顿。在这,笔者直接分享可以用的模型下载地址,如下所示:
下载完成后将后缀为.dae
的文件放进功能包根目录下的meshes
文件夹下,没有的可以自己创建。
3.2代码
def publish_car_model(car_model_pub):
mech_marker = Marker()
mech_marker.header.frame_id = "map"
mech_marker.header.stamp = rospy.Time.now()
# id必须不一样
mech_marker.id = -1
mech_marker.type = Marker.MESH_RESOURCE
mech_marker.action = Marker.ADD
mech_marker.lifetime = rospy.Duration()
mech_marker.mesh_resource = "package://kitti_tutorials/meshes/Car.dae"# .dae模型文件的相对地址
# 位置主要看官方提供的位置图
# 因为自车的velodyne激光雷达相对于map的位置是(0,0,0),看设备安装图上velodyne的位置是(0,0,1.73),显而易见,在rviz中自车模型位置应该是(0,0,-1.73)
mech_marker.pose.position.x = 0.0
mech_marker.pose.position.y = 0.0
mech_marker.pose.position.z = -1.73
# 这边和原作者的代码不一样,因为tf结构发生了一些改变,四元数需要自己根据模型做响应的调整,笔者这边是调整好的模型四元数
q = transformations.quaternion_from_euler(np.pi,np.pi,-np.pi/2.0)
mech_marker.pose.orientation.x = q[0]
mech_marker.pose.orientation.y = q[1]
mech_marker.pose.orientation.z = q[2]
mech_marker.pose.orientation.w = q[3]
mech_marker.color.r = 1.0
mech_marker.color.g = 1.0
mech_marker.color.b = 1.0
mech_marker.color.a = 1.0 # 透明度
# 设置车辆大小
mech_marker.scale.x = 1.0
mech_marker.scale.y = 1.0
mech_marker.scale.z = 1.0
car_model_pub.publish(mech_marker)
3.3效果
最后的效果如下所示:
结语
本文也是基于笔者的学习和使用经验总结的,主观性较强,如果有哪些不对的地方或者不明白的地方,欢迎评论区留言交流~
为了能和读者进一步讨论问题,建立了一个微信群,方便给大家解答问题,也可以一起讨论问题。
加群链接
✌Bye