【从kitti开始自动驾驶】--10.3 本车与其他物体测距(移植至ROS,显示于RVIZ)

最终章!
本节将会将测距代码移植到ROS上,通过RVIZ可视化出来

jupyter工程存放在/test3_autodrive_ws/src/jupyter_prj下的3D_tracking.ipynb

1 程序思路

1. 首先自定义本车车框
2. 其次定义点到直线距离函数 和 两3D框的最短距离
3. 建立发布者pub_dist,写发布函数
4. 对距离直线上进行标注,标注信息发布

2 移植到ROS

kitti_all.py文件中:

2.1 引入本车车框

# 车子8个点的坐标,框,根据车子的数据图示给出
EGOCAR = np.array([[2.15, 0.9, -1.73], [2.15, -0.9, -1.73], [-1.95, -0.9, -1.73], [-1.95, 0.9, -1.73],
                   [2.15, 0.9, -0.23], [2.15, -0.9, -0.23], [-1.95, -0.9, -0.23], [-1.95, 0.9, -0.23]])

2.2 点到距离公式

• 计算点到线的距离的函数
• 该函数返回类型 距离, 坐标

def distance_point_to_segment(P, A, B):
    AP = P - A
    BP = P - B
    AB = B - A
    if np.dot(AB, AP)>=0 and np.dot(-AB, BP)>=0:     #向量点积,投影在线上
        return np.abs(np.cross(AP, AB))/np.linalg.norm(AB), np.dot(AP, AB)/np.dot(AB, AB) * AB + A
    
    d_PA = np.linalg.norm(AP)
    d_PB = np.linalg.norm(BP)
    # 投影在线外
    if d_PA < d_PB:
        return d_PA, A
    return d_PB, B

2.3 每两个3D框距离

• 基于点到直线距离的循环得到
• 输入两个3D框(8X3的矩阵),得到最短距离

def min_distance_cupoints(cub1, cub2):
    minD = 1e5   #先给一个非常大的值
    # 最短距离在本车的端点
    for i in range(4):    #四个顶点遍历
        for j in range(4): #另一个长方形的四条边遍历
            d, Q = distance_point_to_segment(cub1[i, :2], cub2[j, :2], cub2[j+1, :2])
            if d < minD:  #迭代最小的几个点的数值
                minD = d
                minP = EGOCAR[i, :2]
                minQ = Q
                
    # 最短距离在其他物体的端点
    for i in range(4):    #四个顶点遍历
        for j in range(4): #另一个长方形的四条边遍历
            d, Q = distance_point_to_segment(cub2[i, :2], cub1[j, :2], cub1[j+1, :2])
            if d < minD:  #迭代最小的几个点的数值
                minD = d
                minP = corners_3d_velo[i, :2]
                minQ = Q
   
    # 遍历完毕
    return minP, minQ, minD

2.4 建立发布者,且发布

dist_pub = rospy.Publisher("kitti_dist", MarkerArray, queue_size=10) #发布者,发布每两个物体之间的距离
...
publish_dist(dist_pub, minPQDs)
...

2.5 发布函数编写

2.5.1 发布相对距离信息

• 利用markerarray实现同步发送
• 建立marker 的一般信息
• 添加点

for i, (minP, minQ, minD) in enumerate(minPQDs):
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp = rospy.Time.now()

        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration(LIFETIME)
        marker.type = Marker.LINE_STRIP

        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 1.0
        marker.color.a = 0.5
        marker.scale.x = 0.1

        marker.points = []
        marker.points.append(Point(minP[0], minP[1], 0))
        marker.points.append(Point(minQ[0], minQ[1], 0))

        marker_array.markers.append(marker)

2.5.2 发布文本/标注信息

• 在每个距离线段的中心,显示距离数值\
• 一些非常简单的操作便不再赘述

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

        text_marker.id = i + 1000
        text_marker.action = Marker.ADD
        text_marker.lifetime = rospy.Duration(LIFETIME)
        text_marker.type = Marker.TEXT_VIEW_FACING
        #显示文字的位置
        p = (minP + minQ) / 2.0
        text_marker.pose.position.x = p[0]
        text_marker.pose.position.y = p[1]
        text_marker.pose.position.z = 0.0
        #显示文字的内容
        text_marker.text = '%.2f'%minD

        text_marker.scale.x = 1 
        text_marker.scale.y = 1
        text_marker.scale.z = 1

        text_marker.color.r = 1.0
        text_marker.color.g = 1.0
        text_marker.color.b = 1.0
        text_marker.color.a = 0.8
        marker_array.markers.append(text_marker)

3. 效果展示

为rviz添加订阅kitti_dist话题,可看到如下效果,有距离线段和距离

4. 源码

4.1 kitti_all.py

#!/usr/bin/env python3
#coding:utf-8
from data_utils import *
from publish_utils import *
from calibration import *
from collections import deque       #一种数据结构


DATA_PATH = '/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/'
# 车子8个点的坐标,框,根据车子的数据图示给出
EGOCAR = np.array([[2.15, 0.9, -1.73], [2.15, -0.9, -1.73], [-1.95, -0.9, -1.73], [-1.95, 0.9, -1.73],
                   [2.15, 0.9, -0.23], [2.15, -0.9, -0.23], [-1.95, -0.9, -0.23], [-1.95, 0.9, -0.23]])


# 根据长宽高XYZ和旋转角坐标定位
def compute_3d_box_cam2(h, w, l, x, y, z, yaw):
    '''
    return : 3xn in cam2 coordinate
    '''
    
    R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
    x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]
    y_corners = [0, 0, 0, 0, -h, -h, -h, -h]
    z_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2]
    
    corners_3d_cam2 = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))
    corners_3d_cam2 += np.vstack([x, y, z])
    
    return corners_3d_cam2

# 对于每个物体的轨迹
class Object():
    def __init__(self, center):
        self.locations = deque(maxlen=20)   #指定最长长度为20
        self.locations.appendleft(center)   #加入中心位置,第一个位置

    #更新和计算轨迹
    def update(self, center, displacement, yaw):
        #得到当前帧时,所有过去位置的坐标
        for i in range(len(self.locations)):
            x0, y0 = self.locations[i]
            #先旋转,后平移,先乘以旋转矩阵,再添加位移量
            x1 = x0*np.cos(yaw_change) + y0*np.sin(yaw_change) - displacement
            y1 = -x0*np.sin(yaw_change) + y0*np.cos(yaw_change)    
            self.locations[i] = np.array([x1, y1])

        if center is not None:
            self.locations.appendleft(center) #加在左边,最新的位置在左边


    #每一帧结束后,轨迹清空
    def reset(self):
        self.locations = deque(maxlen=20)

# 计算点到线的距离的函数
# 该函数返回类型 距离, 坐标
def distance_point_to_segment(P, A, B):
    AP = P - A
    BP = P - B
    AB = B - A
    if np.dot(AB, AP)>=0 and np.dot(-AB, BP)>=0:     #向量点积,投影在线上
        return np.abs(np.cross(AP, AB))/np.linalg.norm(AB), np.dot(AP, AB)/np.dot(AB, AB) * AB + A
    
    d_PA = np.linalg.norm(AP)
    d_PB = np.linalg.norm(BP)
    # 投影在线外
    if d_PA < d_PB:
        return d_PA, A
    return d_PB, B
        
# 输入两个3D框(8X3的矩阵),得到最短距离
def min_distance_cupoints(cub1, cub2):
    minD = 1e5   #先给一个非常大的值
    # 最短距离在本车的端点
    for i in range(4):    #四个顶点遍历
        for j in range(4): #另一个长方形的四条边遍历
            d, Q = distance_point_to_segment(cub1[i, :2], cub2[j, :2], cub2[j+1, :2])
            if d < minD:  #迭代最小的几个点的数值
                minD = d
                minP = EGOCAR[i, :2]
                minQ = Q
                
    # 最短距离在其他物体的端点
    for i in range(4):    #四个顶点遍历
        for j in range(4): #另一个长方形的四条边遍历
            d, Q = distance_point_to_segment(cub2[i, :2], cub1[j, :2], cub1[j+1, :2])
            if d < minD:  #迭代最小的几个点的数值
                minD = d
                minP = corners_3d_velo[i, :2]
                minQ = Q
   
    # 遍历完毕
    return minP, minQ, minD

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)
    imu_pub = rospy.Publisher("kitti_imu", Imu, queue_size=10)  #IMU发布者
    gps_pub = rospy.Publisher("kitti_gps", NavSatFix, queue_size=10)
    box3d_pub = rospy.Publisher("kitti_3d", MarkerArray, queue_size=10)
    loc_pub = rospy.Publisher("kitti_loc", MarkerArray, queue_size=10)  #发布很多点,所以用markerarray
    dist_pub = rospy.Publisher("kitti_dist", MarkerArray, queue_size=10) #发布者,发布每两个物体之间的距离

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

    rate = rospy.Rate(10)
    #以第0组(厢型车等归类适用的tracking数据)为数据读取
    df_tracking = read_tracking('/home/qinsir/kitti_folder/tracking/data_tracking_label_2/training/label_02/0000.txt')
    calib = Calibration('/home/qinsir/kitti_folder/2011_09_26', from_video=True)

    #ego_car = Object()
    tracker = {}    # track_id: Object,记录所有出现过的物体
    prev_imu_data = None     # 没有第一帧的前一帧的轨迹

    while not rospy.is_shutdown():
        boxes_2d = np.array(df_tracking[df_tracking.frame==frame][["bbox_left", "bbox_top", "bbox_right", "bbox_bottom"]])  #格式转换,第0帧所有的打标框
        types = np.array(df_tracking[df_tracking.frame==frame]["type"])
        boxes_3d = np.array(df_tracking[df_tracking.frame==frame][['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']])
        track_ids = np.array(df_tracking[df_tracking.frame==frame]['track_id'])       #提取每一帧中所有track_id


        
        #保存8个雷达坐标系下点的坐标的list
        corners_3d_velos = []
        #每一个物体都有中心点
        centers = {}    #dict, 记录当前出现的物体
        minPQDs = []    #LIST, 记录每个最小的PQD

        #对于图片中的每一个记录物体都进行计算
        for track_id, box_3d in zip(track_ids, boxes_3d):
            #相机里的坐标
            corners_3d_cam2 = compute_3d_box_cam2(*box_3d)        #用*表展开为7个参数
            #经过矫正到velodyne的函数,入口参数为从cam到velodyne
            #需要8X3,转置一下
            corners_3d_velo = calib.project_ref_to_velo(corners_3d_cam2.T)
            minPQDs += [min_distance_cupoints(EGOCAR, corners_3d_velo)] #对所有车都计算与本车的最短距离
            corners_3d_velos += [corners_3d_velo]           #逐个存入列表
            # 3D盒取平均,获得中心点坐标(不取z)
            centers[track_id] = np.mean(corners_3d_velo, axis=0)[:2] #垂直方向取平均
        centers[-1] = np.array([0, 0]) #加入本车轨迹

        #使用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))
        imu_data = read_imu(os.path.join(DATA_PATH, "oxts/data/%010d.txt"%frame))

        if prev_imu_data is None:
            for track_id in centers:
                tracker[track_id] = Object(centers[track_id]) 
        else :
            displacement = 0.1 * np.linalg.norm(imu_data[['vf', 'vl']])   #得到帧间距离改变量
            yaw_change = float(imu_data.yaw - prev_imu_data.yaw)            #得到帧间角度偏移量
            for track_id in centers:
                if track_id in tracker:
                    tracker[track_id].update(centers[track_id], displacement, yaw_change)
                else:
                    tracker[track_id] = Object(centers[track_id])
            for track_id in tracker:
                if track_id not in centers:
                    tracker[track_id].update(None, displacement, yaw_change)        #不知道中心点在哪儿

        prev_imu_data = imu_data


        publish_camera(cam_pub, bridge, img, boxes_2d, types)
        #publish_camera(cam_pub, bridge, img)
        publish_point_cloud(pcl_pub, point_cloud)
        publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)   #发布三维
        #publish_ego_car(ego_pub)
        #publish_car_model(model_pub)
        publish_two_marker(two_marker_pub)
        publish_imu(imu_pub, imu_data)
        publish_gps(gps_pub, imu_data)
        publish_loc(loc_pub, tracker, centers)
        publish_dist(dist_pub, minPQDs)


        rospy.loginfo('publish frame: %d'%frame)
        rate.sleep()
        frame += 1
        if frame == 154:
            frame = 0
            for track_id in tracker:
                tracker[track_id].reset()

4.2 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, Imu, NavSatFix
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
import cv2

FRAME_ID = "map"
DETECTION_COLOR_DICT = {'Car':(255, 255, 0), 'Pedestrian':(0, 226, 255), 'Cyclist':(141, 40, 255)}
#为三种车型,分别添加颜色值对应的框
LIFETIME = 0.2 
#生存周期

LINES = [[0,1], [1,2], [2,3], [3, 0]]   #底面
LINES += [[4,5], [5,6], [6,7], [7,4]]   #顶面
LINES += [[4,0], [5,1], [6,2], [7,3]]   #中间的四条线
LINES += [[4,1], [5,0]]                 #前边用"X"标志
'''
def publish_camera(cam_pub, bridge, image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image, 'bgr8'))
'''
def publish_camera(cam_pub, bridge, image, boxes, types):
    #画出每一个坐标
    for typ, box in zip(types, boxes):    #for一个矩阵,它是一行一行读取
        # 左上角,右下角,像素都是整数
        top_left = int(box[0]), int(box[1])
        right_down = int(box[2]), int(box[3])
        # 画矩形
        cv2.rectangle(image, top_left, right_down, DETECTION_COLOR_DICT[typ], 2)
    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)

def publish_imu(imu_pub, imu_data):
    # 消息建立
    imu = Imu()

    #头填充
    imu.header.frame_id = FRAME_ID
    imu.header.stamp = rospy.Time.now()

    #旋转角度
    q = tf.transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), float(imu_data.yaw))
    # 将roll, pitch, yaw转成可被电脑识别的四元数q,并设定出去
    imu.orientation.x = q[0]
    imu.orientation.y = q[1]
    imu.orientation.z = q[2]
    imu.orientation.w = q[3]
    #线性加速度
    imu.linear_acceleration.x = imu_data.af
    imu.linear_acceleration.y = imu_data.al
    imu.linear_acceleration.z = imu_data.au 
    #角速度
    imu.angular_velocity.x = imu_data.wf
    imu.angular_velocity.y = imu_data.wl
    imu.angular_velocity.z = imu_data.wu

    #发布
    imu_pub.publish(imu)

def  publish_gps(gps_pub, imu_data):
    gps = NavSatFix()

    #头填充
    gps.header.frame_id = FRAME_ID
    gps.header.stamp = rospy.Time.now()

    #维度, 经度 和海拔
    gps.latitude = imu_data.lat
    gps.longitude = imu_data.lon
    gps.altitude = imu_data.al

    gps_pub.publish(gps)

def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids):
    marker_array = MarkerArray()
    #对每一组数据/交通工具/矩形框进行迭代
    for i, corners_3d_velo in enumerate(corners_3d_velos):

        #加入点云侦测框
        #header部分
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp = rospy.Time.now()

        # marker的id 
        marker.id = i
        marker.action = Marker.ADD      # 加入一个marker
        marker.lifetime = rospy.Duration(LIFETIME)  # 生存时间,()中无参数永远出现
        marker.type = Marker.LINE_STRIP     #marker 的type,有很多种,这里选择线条

        b, g, r = DETECTION_COLOR_DICT[types[i]]

        marker.color.r = r / 255.0
        marker.color.g = g / 255.0
        marker.color.b = b / 255.0           #这条线的颜色

        marker.color.a = 1.0            #透明度 1不透明

        marker.scale.x = 0.1            #大小,粗细

        #设定marker中的资料
        marker.points = []
        #对每两个点之间进行迭代,存在则连
        for l in LINES :
            p1 = corners_3d_velo[l[0]]
            marker.points.append(Point(p1[0], p1[1], p1[2]))
            p2 = corners_3d_velo[l[1]]
            marker.points.append(Point(p2[0], p2[1], p2[2]))
        marker_array.markers.append(marker)

        #加入tracking_id
        #header部分
        text_marker = Marker()
        text_marker.header.frame_id = FRAME_ID
        text_marker.header.stamp = rospy.Time.now()

        # marker的id 
        text_marker.id = i + 1000           #为了和i不重复
        text_marker.action = Marker.ADD      # 加入一个marker
        text_marker.lifetime = rospy.Duration(LIFETIME)  # 生存时间,()中无参数永远出现
        text_marker.type = Marker.TEXT_VIEW_FACING     #marker 的type,有很多种,这里选择text

        p4 = corners_3d_velo[4]
        #以P4点为基准显示文字
        text_marker.pose.position.x = p4[0]
        text_marker.pose.position.y = p4[1]
        text_marker.pose.position.z = p4[2] + 0.5

        text_marker.text = str(track_ids[i])
        #调整字体大小
        text_marker.scale.x = 1 
        text_marker.scale.y = 1
        text_marker.scale.z = 1


        b, g, r = DETECTION_COLOR_DICT[types[i]]

        text_marker.color.r = r / 255.0
        text_marker.color.g = g / 255.0
        text_marker.color.b = b / 255.0           #这条线的颜色

        text_marker.color.a = 1.0            #透明度 1不透明

        text_marker.scale.x = 0.1            #大小,粗细


        marker_array.markers.append(text_marker)


    
    box3d_pub.publish(marker_array)

def publish_loc(loc_pub, tracker, centers):
    #建立markerarray
    marker_array = MarkerArray()

    for track_id in centers:
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp = rospy.Time.now()

        marker.action = Marker.ADD      # 加入一个marker
        marker.lifetime = rospy.Duration(LIFETIME)  # 生存时间,()中无参数永远出现
        marker.type = Marker.LINE_STRIP     #marker 的type,有很多种,这里选择线条
        marker.id = track_id

        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0            #这条线的颜色
        marker.color.a = 1.0            #透明度 1不透明
        marker.scale.x = 0.2            #大小,粗细

        marker.points = []
        #对所有在locations中的点都连接起来
        for p in tracker[track_id].locations:
            marker.points.append(Point(p[0], p[1], 0))

        marker_array.markers.append(marker)

    loc_pub.publish(marker_array)


def publish_dist(dist_pub, minPQDs):
    marker_array = MarkerArray()

    for i, (minP, minQ, minD) in enumerate(minPQDs):
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp = rospy.Time.now()

        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration(LIFETIME)
        marker.type = Marker.LINE_STRIP

        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 1.0
        marker.color.a = 0.5
        marker.scale.x = 0.1

        marker.points = []
        marker.points.append(Point(minP[0], minP[1], 0))
        marker.points.append(Point(minQ[0], minQ[1], 0))

        marker_array.markers.append(marker)

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

        text_marker.id = i + 1000
        text_marker.action = Marker.ADD
        text_marker.lifetime = rospy.Duration(LIFETIME)
        text_marker.type = Marker.TEXT_VIEW_FACING
        #显示文字的位置
        p = (minP + minQ) / 2.0
        text_marker.pose.position.x = p[0]
        text_marker.pose.position.y = p[1]
        text_marker.pose.position.z = 0.0
        #显示文字的内容.取两个百分数
        text_marker.text = '%.2f'%minD

        text_marker.scale.x = 1 
        text_marker.scale.y = 1
        text_marker.scale.z = 1

        text_marker.color.r = 1.0
        text_marker.color.g = 1.0
        text_marker.color.b = 1.0
        text_marker.color.a = 0.8
        marker_array.markers.append(text_marker)

    dist_pub.publish(marker_array)