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

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

Original: https://blog.csdn.net/weixin_36354875/article/details/125935782
Author: Darchan
Title: (ROS_Melodic) 使用Rviz进行Boundingbox可视化