【从kitti开始自动驾驶】--10.3 本车与其他物体测距(移植至ROS,显示于RVIZ)
“勇士低声回应:"我就是风暴”
- 1 程序思路
- 2 移植到ROS
-
- 2.1 引入本车车框
- 2.2 点到距离公式
- 2.3 每两个3D框距离
- 2.4 建立发布者,且发布
- 2.5 发布函数编写
-
- 2.5.1 发布相对距离信息
- 2.5.2 发布文本/标注信息
- 3. 效果展示
- 4. 源码
-
- 4.1 kitti_all.py
- 4.2 publish_utils.py
最终章!
本节将会将测距代码移植到ROS上,通过RVIZ可视化出来
jupyter工程存放在/test3_autodrive_ws/src/jupyter_prj下的3D_tracking.ipynb
1 程序思路
- 首先自定义本车车框
- 其次定义点到直线距离函数 和 两3D框的最短距离
- 建立发布者pub_dist,写发布函数
- 对距离直线上进行标注,标注信息发布
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)