画出track_id
publish_utils.py中
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids):marker_array = MarkerArray()for i, corners_3d_velo in enumerate(corners_3d_velos):marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()marker.id = imarker.action = Marker.ADDmarker.lifetime = rospy.Duration(LIFETIME)marker.type = Marker.LINE_LISTb, g, r = DETECTION_COLOR_DICT[types[i]]marker.color.r = r/255.0marker.color.g = g/255.0marker.color.b = b/255.0marker.color.a = 1.0marker.scale.x = 0.1marker.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)# 添加track_idtext_marker = Marker()text_marker.header.frame_id = FRAME_IDtext_marker.header.stamp = rospy.Time.now()text_marker.id = i + 1000text_marker.action = Marker.ADDtext_marker.lifetime = rospy.Duration(LIFETIME)text_marker.type = Marker.TEXT_VIEW_FACING# 计算中心点的位置p = np.mean(corners_3d_velo, axis=0) # upper front left cornertext_marker.pose.position.x = p[0]text_marker.pose.position.y = p[1]text_marker.pose.position.z = p[2] + 1text_marker.text = str(track_ids[i])text_marker.scale.x = 1text_marker.scale.y = 1text_marker.scale.z = 1b, g, r = DETECTION_COLOR_DICT[types[i]]text_marker.color.r = r/255.0text_marker.color.g = g/255.0text_marker.color.b = b/255.0text_marker.color.a = 1.0marker_array.markers.append(text_marker)box3d_pub.publish(marker_array)
kitti.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import numpy as npimport rospyfrom data_utils import *
from publish_utils import *
from kitti_util import *
import osDATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"def compute_3d_box_cam2(h, w, l, x, y, z, yaw):# 计算旋转矩阵R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])# 8个顶点的xyzx_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]# 旋转矩阵点乘(3，8)顶点矩阵corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))# 加上location中心点，得出8个顶点旋转后的坐标corners_3d_cam2 += np.vstack([x,y,z])return corners_3d_cam2if __name__ == "__main__":frame = 0rospy.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', MarkerArray, queue_size=10)imu_pub = rospy.Publisher('kitti_imu', Imu, queue_size=10)gps_pub = rospy.Publisher('kitti_gps', NavSatFix, queue_size=10)box3d_pub = rospy.Publisher('kitti_3d', MarkerArray, queue_size=10)bridge = CvBridge()df_tracking = read_labelfile('/home/d501//kitti_data/2011_09_26/data_tracking_label_2/training/label_02/0000.txt')calib = Calibration("/home/d501//kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26", from_video=True)rate = rospy.Rate(10)while not rospy.is_shutdown():df_tracking_frame = df_tracking[df_tracking.frame==frame]boxes_2d = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])types = np.array(df_tracking[df_tracking.frame==frame]['type'])boxes_3d = np.array(df_tracking_frame[['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])track_ids = np.array(df_tracking_frame['track id'])corners_3d_velos = []for box_3d in boxes_3d:corners_3d_cam2 = compute_3d_box_cam2(*box_3d)corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)corners_3d_velos += [corners_3d_velo]image = 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))publish_camera(cam_pub, bridge, image, boxes_2d, types)publish_point_cloud(pcl_pub, point_cloud)publish_ego_car(ego_pub)publish_imu(imu_pub, imu_data)publish_gps(gps_pub, imu_data)publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)rospy.loginfo("published")rate.sleep()frame += 1frame %= 154运行结果截图：
画出历史轨迹
画出所有物体历史轨迹
kitti.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import numpy as np
from collections import dequeimport rospyfrom data_utils import *
from publish_utils import *
from kitti_util import *
import osDATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"def compute_3d_box_cam2(h, w, l, x, y, z, yaw):# 计算旋转矩阵R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])# 8个顶点的xyzx_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]# 旋转矩阵点乘(3，8)顶点矩阵corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))# 加上location中心点，得出8个顶点旋转后的坐标corners_3d_cam2 += np.vstack([x,y,z])return corners_3d_cam2# 记录一个物体的历轨迹
class Object():def __init__(self, center):self.locations = deque(maxlen=20)self.locations.appendleft(center)def update(self, center, displacement, yaw_change):for i in range(len(self.locations)):x0, y0 = self.locations[i]x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacementy1 = -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)if __name__ == "__main__":frame = 0rospy.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', MarkerArray, queue_size=10)imu_pub = rospy.Publisher('kitti_imu', Imu, queue_size=10)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)bridge = CvBridge()df_tracking = read_labelfile('/home/d501//kitti_data/2011_09_26/data_tracking_label_2/training/label_02/0000.txt')calib = Calibration("/home/d501//kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26", from_video=True)rate = rospy.Rate(10)tracker = {} # track_id : Objectprev_imu_data = Nonewhile not rospy.is_shutdown():df_tracking_frame = df_tracking[df_tracking.frame==frame]boxes_2d = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])types = np.array(df_tracking[df_tracking.frame==frame]['type'])boxes_3d = np.array(df_tracking_frame[['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])track_ids = np.array(df_tracking_frame['track id'])corners_3d_velos = []# 表示当前帧所有检测到物体的中心centers = {} # track_id : centerfor track_id, box_3d in zip(track_ids, boxes_3d):corners_3d_cam2 = compute_3d_box_cam2(*box_3d)corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)corners_3d_velos += [corners_3d_velo]centers[track_id] = np.mean(corners_3d_velo, axis=0)[:2]image = 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_datapublish_camera(cam_pub, bridge, image, boxes_2d, types)publish_point_cloud(pcl_pub, point_cloud)publish_ego_car(ego_pub)publish_imu(imu_pub, imu_data)publish_gps(gps_pub, imu_data)publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)# 发布历史轨迹publish_loc(loc_pub, tracker, centers)rospy.loginfo("published")rate.sleep()frame += 1if frame == 154:frame = 0for track_id in tracker:tracker[track_id].reset()publish_utils.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import cv2import rospy
from geometry_msgs.msg import Point
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from tf import transformations
from visualization_msgs.msg import Marker, MarkerArray
import numpy as np
import pandas as pd
import os
FRAME_ID = 'map'DETECTION_COLOR_DICT = {'Car': (255, 255, 0), 'Pedestrian': (0, 226, 255), 'Cyclist': (141, 40, 255)}
LIFETIME = 0.1
LINES = [[0, 1], [1, 2], [2, 3], [3, 0]] # Lower plane parallel to Z=0 plane
LINES += [[4, 5], [5, 6], [6, 7], [7, 4]] # Upper plane parallel to Z=0 plane
LINES += [[0, 4], [1, 5], [2, 6], [3, 7]] # Connections between upper and lower planes
LINES += [[4,1], [5,0]]def publish_camera(cam_pub, bridge, image, boxes, types):for type, box in zip(types, boxes):top_left = int(box[0]), int(box[1])bottom_right = int(box[2]), int(box[3])cv2.rectangle(image, top_left, bottom_right, DETECTION_COLOR_DICT[type], 2)cam_pub.publish(bridge.cv2_to_imgmsg(image, 'bgr8'))def publish_point_cloud(pcl_pub, point_cloud):header = Header()header.stamp = rospy.Time.now()header.frame_id = FRAME_IDpcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))def publish_ego_car(ego_car_pub):"""publish left and right 45 degree FOV lines and ego car model mesh:param ego_car_pub::return:"""marker_array = MarkerArray()marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()marker.id = 0marker.action = Marker.ADDmarker.lifetime = rospy.Duration()marker.type = Marker.LINE_STRIPmarker.color.r = 0.0marker.color.g = 1.0marker.color.b = 0.0marker.color.a = 1.0marker.scale.x = 0.2marker.points = []marker.points.append(Point(10, -10, 0))marker.points.append(Point(0, 0, 0))marker.points.append(Point(10, 10, 0))marker_array.markers.append(marker)mech_marker = Marker()mech_marker.header.frame_id = "map"mech_marker.header.stamp = rospy.Time.now()mech_marker.id = -1mech_marker.type = Marker.MESH_RESOURCEmech_marker.action = Marker.ADDmech_marker.lifetime = rospy.Duration()mech_marker.mesh_resource = "package://kitti_tutorial/meshes/Car.dae"mech_marker.pose.position.x = 0.0mech_marker.pose.position.y = 0.0mech_marker.pose.position.z = -1.73q = 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.0mech_marker.color.g = 1.0mech_marker.color.b = 1.0mech_marker.color.a = 1.0mech_marker.scale.x = 1.0mech_marker.scale.y = 1.0mech_marker.scale.z = 1.0marker_array.markers.append(mech_marker)ego_car_pub.publish(marker_array)def publish_imu(imu_pub, imu_data):imu = Imu()imu.header.frame_id = "map"imu.header.stamp = rospy.Time.now()q = transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), float(imu_data.yaw))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.afimu.linear_acceleration.y = imu_data.alimu.linear_acceleration.z = imu_data.auimu.angular_velocity.x = imu_data.wximu.angular_velocity.y = imu_data.wyimu.angular_velocity.z = imu_data.wzimu_pub.publish(imu)def publish_gps(gps_pub, gps_data):gps = NavSatFix()gps.header.frame_id = "map"gps.header.stamp = rospy.Time.now()gps.latitude = gps_data.latgps.longitude = gps_data.longps.altitude = gps_data.altgps_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):marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()marker.id = imarker.action = Marker.ADDmarker.lifetime = rospy.Duration(LIFETIME)marker.type = Marker.LINE_LISTb, g, r = DETECTION_COLOR_DICT[types[i]]marker.color.r = r/255.0marker.color.g = g/255.0marker.color.b = b/255.0marker.color.a = 1.0marker.scale.x = 0.1marker.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)# 添加track_idtext_marker = Marker()text_marker.header.frame_id = FRAME_IDtext_marker.header.stamp = rospy.Time.now()text_marker.id = i + 1000text_marker.action = Marker.ADDtext_marker.lifetime = rospy.Duration(LIFETIME)text_marker.type = Marker.TEXT_VIEW_FACING# 计算中心点的位置p = np.mean(corners_3d_velo, axis=0) # upper front left cornertext_marker.pose.position.x = p[0]text_marker.pose.position.y = p[1]text_marker.pose.position.z = p[2] + 1text_marker.text = str(track_ids[i])text_marker.scale.x = 1text_marker.scale.y = 1text_marker.scale.z = 1b, g, r = DETECTION_COLOR_DICT[types[i]]text_marker.color.r = r/255.0text_marker.color.g = g/255.0text_marker.color.b = b/255.0text_marker.color.a = 1.0marker_array.markers.append(text_marker)box3d_pub.publish(marker_array)# 发布历史轨迹
def publish_loc(loc_pub, tracker, centers):marker_array = MarkerArray()for track_id in centers:marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()marker.action = Marker.ADDmarker.lifetime = rospy.Duration(LIFETIME)marker.type = Marker.LINE_STRIPmarker.id = track_idmarker.color.r = 1.0marker.color.g = 0.0marker.color.b = 0.0marker.color.a = 1.0marker.scale.x = 0.2marker.points = []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 distance_point_to_segment(P, A, B):"""Calculate the min distance of a point P to a segment AB.Return the point Q in AB on which the min distance is reached.:param P::param A::param B::return:"""AP = P - ABP = P - BAB = 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, Areturn d_PB, Bdef min_distance_cuboids(cub1, cub2):"""Computes the minimum distance between two non-overlapping cuboids (3D) of shape (8, 3)They are projected to BEV and the minium distance of the two rectangles are returned:param cub1::param cub2::return:"""minD = 1e5for 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 = dminP = cub1[i, :2]minQ = Qfor 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 = dminP = cub2[i, :2]minQ = Qreturn minP, minQ, minD修改kitti.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import numpy as np
from collections import dequeimport rospyfrom data_utils import *
from publish_utils import *
from kitti_util import *
import osDATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"
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]])def compute_3d_box_cam2(h, w, l, x, y, z, yaw):# 计算旋转矩阵R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])# 8个顶点的xyzx_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]# 旋转矩阵点乘(3，8)顶点矩阵corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))# 加上location中心点，得出8个顶点旋转后的坐标corners_3d_cam2 += np.vstack([x,y,z])return corners_3d_cam2# 记录一个物体的历轨迹
class Object():def __init__(self, center):self.locations = deque(maxlen=20)self.locations.appendleft(center)def update(self, center, displacement, yaw_change):for i in range(len(self.locations)):x0, y0 = self.locations[i]x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacementy1 = -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)if __name__ == "__main__":frame = 0rospy.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', MarkerArray, queue_size=10)imu_pub = rospy.Publisher('kitti_imu', Imu, queue_size=10)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)# 显示车距dist_pub = rospy.Publisher('kitti_dist', MarkerArray, queue_size=10)bridge = CvBridge()df_tracking = read_labelfile('/home/d501//kitti_data/2011_09_26/data_tracking_label_2/training/label_02/0000.txt')calib = Calibration("/home/d501//kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26", from_video=True)rate = rospy.Rate(10)# 所有物体tracker = {} # track_id : Objectprev_imu_data = Nonewhile not rospy.is_shutdown():df_tracking_frame = df_tracking[df_tracking.frame==frame]boxes_2d = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])types = np.array(df_tracking[df_tracking.frame==frame]['type'])boxes_3d = np.array(df_tracking_frame[['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])track_ids = np.array(df_tracking_frame['track id'])corners_3d_velos = []# 表示当前帧所有检测到物体的中心centers = {} # track_id : center# 最短距离和点minPQDs = []for track_id, box_3d in zip(track_ids, boxes_3d):corners_3d_cam2 = compute_3d_box_cam2(*box_3d)corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)minPQDs.append(min_distance_cuboids(EGOCAR, corners_3d_velo))corners_3d_velos += [corners_3d_velo]centers[track_id] = np.mean(corners_3d_velo, axis=0)[:2]corners_3d_velos.append(EGOCAR)types = np.append(types, 'Car')track_ids = np.append(track_ids, -1)# 表示汽车本身centers[-1] = np.array([0, 0])image = 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_datapublish_camera(cam_pub, bridge, image, boxes_2d, types)publish_point_cloud(pcl_pub, point_cloud)publish_ego_car(ego_pub)publish_imu(imu_pub, imu_data)publish_gps(gps_pub, imu_data)publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)# 发布历史轨迹publish_loc(loc_pub, tracker, centers)publish_dist(dist_pub, minPQDs)rospy.loginfo("published")rate.sleep()frame += 1if frame == 154:frame = 0for track_id in tracker:tracker[track_id].reset()publish_utils.py添加函数
def publish_dist(dist_pub, minPQDs):marker_array = MarkerArray()for i, (minP, minQ, minD) in enumerate(minPQDs):marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()marker.action = Marker.ADDmarker.lifetime = rospy.Duration(LIFETIME)marker.type = Marker.LINE_STRIPmarker.id = imarker.color.r = 1.0marker.color.g = 0.0marker.color.b = 1.0marker.color.a = 0.5marker.scale.x = 0.1marker.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_IDtext_marker.header.stamp = rospy.Time.now()text_marker.id = i + 1000text_marker.action = Marker.ADDtext_marker.lifetime = rospy.Duration(LIFETIME)text_marker.type = Marker.TEXT_VIEW_FACINGp = (minP + minQ) / 2.0text_marker.pose.position.x = p[0]text_marker.pose.position.y = p[1]text_marker.pose.position.z = 0text_marker.text = '%.2f'%minDtext_marker.scale.x = 1text_marker.scale.y = 1text_marker.scale.z = 1text_marker.color.r = 1.0text_marker.color.g = 1.0text_marker.color.b = 1.0text_marker.color.r = 0.0marker_array.markers.append(text_marker)dist_pub.publish(marker_array)
![[计算机网络]--IP协议](https://img-blog.csdnimg.cn/direct/fb0769f9c27e47fab3a7ddca05213887.png)
