【从kitti开始自动驾驶】--3.2代码优化-同步发送

本节将在 3.1节的基础上,进行代码优化功能不变

1. 提出问题

mark相机视野和mark车的模型,从发布,到定义,都存在着时间差,能不能让他们同步发送
答案是有的,利用我们的MakerArray把它们都添加进来然后一起发送

2.改进

在publish_utils中:
将原来的两个publish mark的函数合并为一个markerarray.

from visualization_msgs.msg importMarkerArray

• 引入头文件

---

新写函数:publish_two_marker()
主要是将原来两个分开的mark 合并发送,关键代码如下:

from visualization_msgs.msg importMarkerArray

---

其他代码直接复制,去掉单独pulish,
改为每一次数据填充完毕后放入makerarray

markerarray.markers.append(marker)
...
markerarray.markers.append(mesh_marker)

---

全部添加完毕以后再发送

kitti_two_marker.publish(markerarray)

---

在kitti_all.py中要改的代码是:
取消两个publish的初始化和发布,改为用新的函数

#ego_pub = rospy.Publisher('kitti_ego_car', Marker, queue_size=10)
#model_pub = rospy.Publisher("kitti_car_model", Marker, queue_size=10)
two_marker_pub = rospy.Publisher("kitti_two_mark", MarkerArray, queue_size=10)
......
......
#publish_ego_car(ego_pub)
#publish_car_model(model_pub)
publish_two_marker(two_marker_pub)

---

这样的话,它就同时发布了

3. 源码

publish_utils.py

#!/usr/bin/env python3
#coding:utf-8

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2
from visualization_msgs.msg import Marker, MarkerArray
import sensor_msgs.point_cloud2 as pcl2
from geometry_msgs.msg import Point
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_cloud):
    header = Header()
    header.frame_id = FRAME_ID
    header.stamp = rospy.Time.now()
    pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))


def publish_ego_car(ego_car_pub):
    'publish left and right 45 degree FOV and ego car model mesh'
    #header部分
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()
    # marker的id 
    marker.id = 0
    marker.action = Marker.ADD      # 加入一个marker
    marker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现
    marker.type = Marker.LINE_STRIP     #marker 的type,有很多种,这里选择线条

    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            #大小,粗细

    #设定marker中的资料
    marker.points = []
    # 两条线,三个点即可
    #原点是0,0,0, 看左上角和右上角的数据要看kitti的设定,看坐标
    marker.points.append(Point(10, -10, 0))
    marker.points.append(Point(0, 0, 0))
    marker.points.append(Point(10, 10, 0))

    ego_car_pub.publish(marker) #设定完毕,发布

def publish_car_model(model):
    #header部分
    mesh_marker = Marker()
    mesh_marker.header.frame_id = FRAME_ID
    mesh_marker.header.stamp = rospy.Time.now()
    # marker的id 
    mesh_marker.id = -1
    mesh_marker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现
    mesh_marker.type = Marker.MESH_RESOURCE     #marker 的type,有很多种,这里选择mesh
    mesh_marker.mesh_resource = "package://demo1_kitti_pub_photo/mesh/car_model/car.DAE"

    #平移量设置
    mesh_marker.pose.position.x = 0.0
    mesh_marker.pose.position.y = 0.0
    #以为0,0,0 是velodyne的坐标(车顶),这里坐标是车底,所以是负数
    mesh_marker.pose.position.z = -1.73

    #旋转量设定
    q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi/2)
    # 这里的参数和下载模型的预设角度有关,旋转关系,根据显示效果而调整,转成四元数q
    #x轴旋转
    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

    #设置体积:  x,y,z方向的膨胀程度
    mesh_marker.scale.x = 0.4
    mesh_marker.scale.y = 0.4
    mesh_marker.scale.z = 0.4

    model.publish(mesh_marker) #设定完毕,发布

def publish_two_marker(kitti_two_marker):
    #建立markerarray
    markerarray = MarkerArray()
    # 绿线设定
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()
    # marker的id 
    marker.id = 0
    marker.action = Marker.ADD      # 加入一个marker
    marker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现
    marker.type = Marker.LINE_STRIP     #marker 的type,有很多种,这里选择线条

    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            #大小,粗细

    #设定marker中的资料
    marker.points = []
    # 两条线,三个点即可
    #原点是0,0,0, 看左上角和右上角的数据要看kitti的设定,看坐标
    marker.points.append(Point(10, -10, 0))
    marker.points.append(Point(0, 0, 0))
    marker.points.append(Point(10, 10, 0))
    #加入第一个
    markerarray.markers.append(marker)

    mesh_marker = Marker()
    mesh_marker.header.frame_id = FRAME_ID
    mesh_marker.header.stamp = rospy.Time.now()
    # marker的id 
    mesh_marker.id = -1
    mesh_marker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现
    mesh_marker.type = Marker.MESH_RESOURCE     #marker 的type,有很多种,这里选择mesh
    mesh_marker.mesh_resource = "package://demo1_kitti_pub_photo/mesh/car_model/car.DAE"

    #平移量设置
    mesh_marker.pose.position.x = 0.0
    mesh_marker.pose.position.y = 0.0
    #以为0,0,0 是velodyne的坐标(车顶),这里坐标是车底,所以是负数
    mesh_marker.pose.position.z = -1.73

    #旋转量设定
    q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi/2)
    # 这里的参数和下载模型的预设角度有关,旋转关系,根据显示效果而调整,转成四元数q
    #x轴旋转
    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

    #设置体积:  x,y,z方向的膨胀程度
    mesh_marker.scale.x = 0.4
    mesh_marker.scale.y = 0.4
    mesh_marker.scale.z = 0.4
    # 加入第二个:车子模型
    markerarray.markers.append(mesh_marker)

    #发布
    kitti_two_marker.publish(markerarray)

kitti_all.py

#!/usr/bin/env python3
#coding:utf-8
from data_utils import *
from publish_utils import *


DATA_PATH = '/home/qinsir/kitti_folder/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)
    pcl_pub = rospy.Publisher("kitti_point_cloud", PointCloud2, queue_size=10)
    #ego_pub = rospy.Publisher('kitti_ego_car', Marker, queue_size=10)
    #model_pub = rospy.Publisher("kitti_car_model", Marker, queue_size=10)
    two_marker_pub = rospy.Publisher("kitti_two_mark", MarkerArray, queue_size=10)

    bridge = CvBridge()      #opencv支持的图片和ROS可以读取的图片之间转换的桥梁

    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        #使用OS,路径串接,%010d,这个字串有10个数字(比如0000000001).png
        img = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
        point_cloud = read_point_cloud(os.path.join(DATA_PATH, "velodyne_points/data/%010d.bin"%frame))
        
        publish_camera(cam_pub, bridge, img)
        publish_point_cloud(pcl_pub, point_cloud)
        #publish_ego_car(ego_pub)
        #publish_car_model(model_pub)
        publish_two_marker(two_marker_pub)

        rospy.loginfo('new file publish')
        rate.sleep()
        frame += 1
        frame %= 154