python:利用rospy实现Marker及XYZI/XYZRGB点云在rviz中的可视化

应用背景

在做点云深度学习的时候，需要对分割或者检测结果进行可视化，可能是不同label的点加上不同颜色的显示、也可能是不同检测目标需要添加不同的box或者不同颜色的显示，总之需求多种多样，目的也是为了给结果更鲜明的展示出来。
基于这样的目的需求，这里提供一种方案，利用ros中rviz可视化工具来显示，个人比较喜欢rviz的可视化效果，比open3d要情有独钟。下面会把详细的示例代码给出来，主要是带有颜色或者强度信息的点云可视化，以及marker的线条可视化（个人在做车道线显示都是这种方式）。

依赖环境

这里要说明一下，代码环境是基于ubuntu+ros的
需要安装rospy、numpy等

示例代码

可以直接作为工程里的一个脚本，在自己代码中import使用

# coding=utf-8
import rospy
import pcl_ros
import time
import numpy as np
from std_msgs.msg import ColorRGBA
from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2, PointField
from sensor_msgs import point_cloud2 as pc2
# from custom_msgs.msg import PointXYZIRGB
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point


class Rviz():
    def __init__(self, points,
                colors=None,
                frame_id='rslidar',
                topic='/lane_points'):
        rospy.init_node('rviz_node', anonymous=True)
        self.pub_topic = topic
        self.pub_cloud = rospy.Publisher(self.pub_topic, PointCloud2, queue_size=10)    
        self.pub_markers = rospy.Publisher('/lane_markers', MarkerArray, queue_size=10)  
        self.rate = rospy.Rate(10)       
        self.header = Header()
        self.header.frame_id = frame_id
        self.header.stamp = rospy.Time().now() # 或者time.time() # 获取当前时刻微秒级时间戳
        self.points = points
        self.colors = colors
        rospy.loginfo('rospy init...')

    def xyzrgb_to_pointcloud2(self, points):
        msg = PointCloud2()
        assert(points.shape[1] == 6)
        msg.header = self.header
        if len(self.points.shape) == 3:
            msg.height = self.points.shape[1]
            msg.width = s