(ROS_Melodic) 使用Rviz进行Boundingbox可视化
先赞后看,养成好习惯。
有帮助的话,点波关注!
我会坚持更新,感谢您的支持!
1. 需求
通过Rviz可视化障碍物的包围盒,将点云与boundingbox绘制在一起,方便检查标注框是否准确。
最后列举一个可能碰到的在Rviz中boundingbox无法显示
的问题。
2. 环境
Ubuntu18.04、ROS(melodic)
3. 参考
1. rviz显示矩形框BoundingBox
2. 无人驾驶汽车系统入门(二十五)——基于欧几里德聚类的激光雷达点云分割及ROS实现
4. 安装依赖和插件
sudo apt-get install ros-melodic-jsk-recognition-msgs & sudo apt-get install ros-melodic-jsk-rviz-plugins
sudo apt-get install ros-melodic-jsk-rqt-plugins
sudo apt-get install ros-melodic-jsk-visualization
5. Rviz添加topic
在Rviz左侧的DIsplay面板下,点击Add
,在By_display_type选项卡中,选择BoundingBoxArray
。
6. BoundingBoxArray发布程序
参考样例,主要填写BoundingBox中的各类属性即可,主要包含header、dimensions、label、value等。
#!/usr/bin/env python
# coding=utf-8
import os
import numpy as np
from torch import float32
import rospy
from visualization_msgs.msg import *
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2 as pc2
from jsk_recognition_msgs.msg import BoundingBoxArray, BoundingBox
import signal
file_path = '/home/cui/data/motion_seg_ht/ht_data1/raw_data/'
label_path = '/home/cui/data/motion_seg_ht/ht_data1/box_label/'
flag_auto_play = 0 # 1为自动播放, 0为单帧播放
# 信号捕获
def term_sig_handler(signum, frame):
print('catched singal: %d' % signum)
sys.exit()
# 读取label文件,获取中心和长宽高
def readTxt(data_path, bboxes):
# center: n*3 , n代表box数目
# size: n*3 , n代表box数目
bboxes.boxes = []
instance_num = len(np.array(open(data_path,'rU').readlines()))
if instance_num == 0:
return bboxes
with open(data_path,"r") as f:
for line in f.readlines():
line = line.strip("\n")
line = line.split()
# 中心,长宽属性
center = (np.array(line))[18:21]
size = (np.array(line))[9:12] #高宽长
# 建立标注框
box = BoundingBox()
box.header.frame_id = 'rslidar'
box.pose.position.x = float(center[0])
box.pose.position.y = float(center[1])
box.pose.position.z = float(center[2])
box.pose.orientation.w = 1
box.pose.orientation.x = 0
box.pose.orientation.y = 0
box.pose.orientation.z = 0
box.dimensions.x = float(size[2])
box.dimensions.y = float(size[1])
box.dimensions.z = float(size[0])
box.label = 1
box.value = 0
bboxes.boxes.append(box)
return bboxes
# 获取文件中点云数据
def get_data(file_path, file_extension):
file_name = []
timestamp_list = []
for filename in os.listdir(file_path):
timestamp = filename.split(file_extension)[0]
timestamp_list.append(timestamp)
timestamp_list.sort()
for filename in timestamp_list:
file_name.append(str(filename) + file_extension)
# print(file_name)
return file_name
def main():
signal.signal(signal.SIGINT, term_sig_handler) #ctrl -c
rospy.init_node("point_cloud", anonymous=True)
rate = rospy.Rate(10)
pub_cloud = rospy.Publisher("/point_cloud", PointCloud2, queue_size=100)
pub_boxes = rospy.Publisher("/bounding_boxes", BoundingBoxArray, queue_size=100)
point_cloud2 = PointCloud2()
point_cloud2.header.frame_id = "rslidar"
bboxes = BoundingBoxArray()
bboxes.header.frame_id = "rslidar"
file_name = get_data(file_path, '.bin')
# 连续发布
for file in file_name:
# 加载点云并发布
point_data = np.fromfile((file_path + str(file)), dtype=np.float32, count=-1).reshape([-1, 4])
cloud = pc2.create_cloud_xyzi32(point_cloud2.header, point_data[:, :4])
pub_cloud.publish(cloud)
# 加载标注框,并发布
label_file = file.split('.bin')[0] + '.txt'
bboxes = readTxt(label_path + label_file, bboxes)
pub_boxes.publish(bboxes)
if flag_auto_play == 1:
rate.sleep()
else:
n = input("please click enter, goto next frame!")
if __name__ == "__main__":
main()
7. 可视化效果
8. 实际运行过程中碰到一个bbox无法显示的bug
- 情况说明:总共200个box,但是可视化只显示一个box,其余199个均不显示。同时Rviz的命令行出现Invalid size of bounding box is included and skipped: [2.242119, 0.917433, -2.503252]
- 问题原因:从第2个box开始,box的size.z()
中出现了负值,故从第2个开始,其余的199个box均不显示了。这里感觉插件做的不够鲁棒,应该只对出现问题的进行跳过,其余size正常的进行正常显示。
- 解决办法: 在size赋值时,变为正值。
bbox.dimensions.x = fabs(size.x());
bbox.dimensions.y = fabs(size.y());
bbox.dimensions.z = fabs(size.z());