(ROS_Melodic) 使用Rviz进行Boundingbox可视化

原创已于 2022-08-12 17:00:39 修改 · 4.2k 阅读

25 ·

CC 4.0 BY-SA版权

文章标签：

#python #深度学习 #机器学习 #目标检测 #自动驾驶

于 2022-07-22 17:00:56 首次发布

工程化技巧专栏收录该内容

9 篇文章

订阅专栏

该博客介绍了如何在ROS Melodic环境下，利用Rviz进行点云数据的包围盒（Boundingbox）可视化。首先，安装必要的jsk插件，然后在Rviz中添加BoundingBoxArray显示。接着，编写发布BoundingBoxArray的Python程序，从点云数据和标注文件中读取信息。在实际运行中，遇到问题：当box尺寸的z轴值为负时，Rviz无法显示除第一个外的其他box。解决方法是确保尺寸值为正。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

(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());