[可视化] rviz的可视化（python）

1. ros jsk_recognition_msgs 中BoundingBox绘制（目前只有ros1，ros2中无组件）

• TODO

2. ros marker 绘制bounding box (适用ros1, ros2)

uint8 ARROW=0 
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8
uint8 TEXT_VIEW_FACING=9 //绘制文字
uint8 MESH_RESOURCE=10
uint8 TRIANGLE_LIST=11

uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3  # 用于清除上一次发出的markers, 因为marker特性是一致保留，不清除，会导致上下帧之间对象共存

2.1 利用LINE绘制box

• LINE_LIST 方式
  知乎-ros marker 绘制box

注意通过marker 的LINE绘制的box框某些角度会导致线条消失，可视化效果不是很完美

• LINE_STRIP方式

import numpy as np
import rclpy
from rclpy.node import Node

from visualization_msgs.msg import MarkerArray, Marker
from std_msgs.msg import Header
from geometry_msgs.msg import Point

pub = create_publisher(MarkerArray, "box", 10)

# code refs: OpenPCDet (mmlab)
def boxes_to_corners_3d(boxes3d):
    """
      7 -------- 4
     /|         /|
    6 -------- 5 .
    | |        | |
    . 3 -------- 0
    |/         |/
    2 -------- 1
    Args:
        boxes3d:  (N, 8) cls_id, x,y,z,l,w,h,angle[-pi,pi], (x, y, z) is the box center

    Returns:
        corners3d: (N, 8, 3)
    """
    boxes3d, is_numpy = common_utils.check_numpy_to_torch(boxes3d)

    template = boxes3d.new_tensor((
        [1, 1, -1], [1, -1, -1], [-1, -1, -1], [-1, 1, -1],
        [1, 1, 1], [1, -1, 1], [-1, -1, 1], [-1, 1, 1],
    )) / 2

    corners3d = boxes3d[:, None, 4:7].repeat(1, 8, 1) * template[None, :, :]
    corners3d = common_utils.rotate_points_along_z(corners3d.view(-1, 8, 3), boxes3d[:, 7]).view(-1, 8, 3)
    corners3d += boxes3d[:, None, 1:4]

    return corners3d.numpy() if is_numpy else corners3d

def pub_my_boxes(bboxes, topic='gt', color=(1.0,0.0,0.0,1.0), line_width=0.1):
    """
    Args:
        bboxes[np.ndarray]:(M, 7)
        topic[str]:ros topic 
        color[tuple]: (R,G,B,A)
        line_width: width of line of bouding box = marker.scale.x
    """

    # clear last time markers
    pub.publish(markerD)  # 2.5 章节介绍
    
    bboxes = boxes_to_corners_3d(bboxes)  # 获取8个点位置信息

    mArr = MarkerArray()
    for i, box in enumerate(bboxes):
        marker = Marker()
        marker.header.frame_id = 'map'
        marker.id = i  # must be unique
        marker.type = Marker.LINE_STRIP
        marker.action = Marker.ADD
        marker.points = [Point(x=float(b[0]), y=float(b[1]), z=float(b[2])) for b in box[:4]]
        marker.points.append(Point(x=float(box[0,0]), y=float(box[0,1]), z=float(box[0,2])))
        marker.points += [Point(x=float(b[0]), y=float(b[1]), z=float(b[2])) for b in box[4:]]
        marker.points.append(Point(x=float(box[4,0]), y=float(box[4,1]), z=float(box[4,2])))
        temp = [5,1,2,6,7,3]
        marker.points += [Point(x=float(box[i, 0]), y=float(box[i, 1]), z=float(box[i, 2])) for i in temp]

        marker.scale.x = float(line_width)

        marker.color.r = float(color[0])
        marker.color.g = float(color[1])
        marker.color.b = float(color[2])
        marker.color.a = float(color[3])

        mArr.markers.append(marker)

    pub.publish(mArr)

2.2 利用CUBE绘制box

主要设置半透明，否则看不出来里面的点

# 参考他人代码，用于将heading表示为四元数， 正在绘制box的时候，赋值给pos.orientation
_AXES2TUPLE = {
    'sxyz': (0,0,0,0), 'sxyx': (0,0,1,0), 'sxzy':(0,1,0,0),
    'sxzx': (0,1,1,0), 'syzx': (1,0,0,0), 'syzy':(1,0,1,0),
    'syxz': (1,1,0,0), 'syxy': (1,1,1,0), 'szxy':(2,0,0,0),
    'szxz': (2,0,1,0), 'szyx': (2,1,0,0), 'szyz':(2,1,1,0),
    'rzyx': (0,0,0,1), 'rxyx': (0,0,1,1), 'ryzx':(0,1,0,1),
    'rxzx': (0,1,1,1), 'rxzy': (1,0,0,1), 'ryzy':(1,0,1,1),
    'rzxy': (1,1,0,1), 'ryxy': (1,1,1,1), 'ryxz':(2,0,0,1),
    'rzxz': (2,0,1,1), 'rxyz': (2,1,0,1), 'rzyz':(2,1,1,1),
}

_NEXT_AXIS = [1,2,0,1]

def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
    try:
        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
    except (AttributeError, KeyError):
        # _ = _TUPLE2AXES[axes]
        firstaxis, parity, repetition, frame = axes
    i = firstaxis
    j = _NEXT_AXIS[i+parity]
    k = _NEXT_AXIS[i - parity + 1]

    if frame:
        ai, ak = ak, ai
    
    if parity:
        aj = -aj

    ai /= 2.0
    aj /= 2.0
    ak /= 2.0 
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci * ck
    cs = ci * sk
    sc = si * ck
    ss = si * sk

    quaternion = np.empty((4, ), dtype=np.float64)
    if repetition:
        quaternion[i] = cj * (cs + sc)
        quaternion[j] = sj * (cc + ss)
        quaternion[k] = sj * (cs - sc)
        quaternion[3] = cj * (cc - ss)
    else:
        quaternion[i] = cj * sc - sj * cs
        quaternion[j] = cj * ss + sj * cc
        quaternion[k] = cj * cs - sj * sc
        quaternion[3] = cj * cc + sj * ss
    if parity:
        quaternion[j] *= -1

    return quaternion

def pub_my_boxes(bboxes, topic='box', color=(1.0,0.0,0.0, 0.3)):
    """
    Args:
        bboxes[np.ndarray]:(M, 8) cls_id, x,y,z,l,w,h,angle[-pi,pi]
        topic[str]: ros topic
        color[tuple]: (r,g,b,a)
    """
    # clear last time markers
    pub.publish(self.markerD)

    mArr = MarkerArray()
    for i, box in enumerate(bboxes):
        marker = Marker()
        marker.header.frame_id = 'map'
        marker.id = i  # must be unique
        marker.type = Marker.CUBE
        marker.action = Marker.ADD
        marker.pose.position.x = float(box[1])
        marker.pose.position.y = float(box[2])
        marker.pose.position.z = float(box[3])
        # 用四元数设置朝向
        q = quaternion_from_euler(0, 0, box[7])
        marker.pose.orientation.x = q[0]
        marker.pose.orientation.y = q[1]
        marker.pose.orientation.z = q[2]
        marker.pose.orientation.w = q[3]

        marker.scale.x = float(box[4])
        marker.scale.y = float(box[5])
        marker.scale.z = float(box[6])

        marker.color.r = float(color[0])
        marker.color.g = float(color[1])
        marker.color.b = float(color[2])
        marker.color.a = float(color[3])

        mArr.markers.append(marker)

2.3 利用arrow绘制heading

其他代码与CUBE绘制box 类似，此处省略

... ...

marker_arrow = Marker()
marker_arrow.header.frame_id = 'map'
marker_arrow.id = i + offset_id  # must be unique for all of markers
marker_arrow.type = Marker.ARROW
marker_arrow.action = Marker.ADD
# base position of arrow; 在框的正前方面中心
marker_arrow.pose.position.x = float(box[1]) + float(box[4]) / 2.0 * math.cos(box[7])
marker_arrow.pose.position.y = float(box[2]) + float(box[4]) / 2.0 * math.sin(box[7])
marker_arrow.pose.position.z = float(box[3])

marker_arrow.scale.x = 0.1 # 箭柄
marker_arrow.scale.y = 0.2 # 箭头
marker_arrow.scale.z = 0.0
marker_arrow.points.clear()
start_p = Point()
end_p = Point()
length = 1.0   # length of box 

# 相对与base position的
start_p.x = 0.0
start_p.y = 0.0
end_p.x = length * math.cos(box[7])
end_p.y = length * math.sin(box[7])

marker_arrow.points.append(start_p)
marker_arrow.points.append(end_p)

marker_arrow.color.r = 1.0
marker_arrow.color.g = 1.0
marker_arrow.color.b = 1.0 
marker_arrow.color.a = 1.0

marker_arrow.pose.orientation.w = 1.0
mArr.markers.append(marker_arrow)

2.4 利用text绘制box’s score

可以设置小数点精度

... ...
marker_txt = Marker()
marker_txt.header.frame_id = 'map'
marker_txt.id = offest_txt_id + i  # must be unique
marker_txt.type = Marker.TEXT_VIEW_FACING
marker_txt.action = Marker.ADD
# 框中心顶部
marker_txt.pose.position.x = float(box[1])
marker_txt.pose.position.y = float(box[2])
marker_txt.pose.position.z = float(box[3] + box[6] / 2.0)
marker_txt.color.r = 1.0
marker_txt.color.g = 1.0
marker_txt.color.b = 1.0 
marker_txt.color.a = 1.0
marker_txt.scale.z = 0.3  # size of text
marker_txt.text = label  # str
mArr.markers.append(marker_txt)

2.5 利用DELETEALL实现实时刷新

注意如果不清除上一次发送的marker对象，下一帧发送的会与上一帧重叠，无法实现实时每帧刷新的效果

markerD = MarkerArray()
marker = Marker()
marker.action = Marker.DELETEALL  # 会清除同一个通道中所有的markers
markerD.markers.append(marker)

2.6 运行ros2 测试示例 (in ros2)

可视化box的简单实例，需要集成Node，这是ros2的写法，ros1有一点不同

import rclpy
from rclpy.node import Node

from visualization_msgs.msg import MarkerArray, Marker
# from std_msgs.msg import Header


class MyPublisher(Node):
    def __init__(self, node_name='my_publisher'):
       super().__init__(node_name)
       self.pub = self.create_publisher(MarkerArray, "gt", 10)
       self.markerD = MarkerArray()
       marker = Marker()
       marker.action = Marker.DELETEALL
       self.markerD.markers.append(marker)

    def publish_boxes(self, bboxes, topic='box', color=(1.0,0.0,0.0, 0.3)):
        """
        Args:
            bboxes[np.ndarray]:(M, 8) xyzlwh heading
            topic[str]: ros topic
            color[tuple]: (r,g,b,a)
        """
        # clear last time markers
        self.pub.publish(self.markerD)

        mArr = MarkerArray()
        for i, box in enumerate(bboxes.astype(np.float32)):
            marker = Marker()
            marker.header.frame_id = self.header.frame_id
            marker.id = i  # must be unique
            marker.type = Marker.CUBE
            marker.action = Marker.ADD
            marker.pose.position.x = float(box[1])
            marker.pose.position.y = float(box[2])
            marker.pose.position.z = float(box[3])
            q = quaternion_from_euler(0, 0, box[7])  # 上面有写
            marker.pose.orientation.x = q[0]
            marker.pose.orientation.y = q[1]
            marker.pose.orientation.z = q[2]
            marker.pose.orientation.w = q[3]

            marker.scale.x = float(box[4])
            marker.scale.y = float(box[5])
            marker.scale.z = float(box[6])

            marker.color.r = float(color[0])
            marker.color.g = float(color[1])
            marker.color.b = float(color[2])
            marker.color.a = float(color[3])

            mArr.markers.append(marker)

        self.pub.publish(mArr)

if __name__ == '__main__':
    rclpy.init()  # 必须有
    pub = MyPublisher("test_ros_pub")
  
    for i in range(30):
        bboxes = np.load(f"../box.npy")
        bboxes = np.concatenate([np.zeros((bboxes.shape[0],1), dtype=np.float32), bboxes], axis=1)
        pub.publish_boxes2(bboxes, 'box')
        time.sleep(1)

    rclpy.shutdown()

3. 绘制points

3.1 不带反射强度（intensity）

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import PointCloud2, PointField
from sensor_msgs_py import point_cloud2
from std_msgs.msg import Header



header = Header()
header.frame_id = 'map'

dtype = PointField.FLOAT32

xyz_fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
          PointField(name='y', offset=4, datatype=dtype, count=1),
          PointField(name='z', offset=8, datatype=dtype, count=1)]

def publish(points, topic='point_cloud'):
    """
    Args:
        points: (N, 3), 
        topic: string, 
    """

    fields = xyz_fields

    header.stamp = self.get_clock().now().to_msg()
    # points （N,3）
    pc2_msg = point_cloud2.create_cloud(header, fields, points)
    self.pub_p.publish(pc2_msg)

3.2 带反射强度

主要用于对于感知过程中，一些点的感知权重显示（ 将weights设置到intensity通道上），是否符合预期

# 将xyz_fields 换成xyzi_fields，并且保证points(N,4）
xyzi_fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
          PointField(name='y', offset=4, datatype=dtype, count=1),
          PointField(name='z', offset=8, datatype=dtype, count=1),
          PointField(name='intensity', offset=12, datatype=dtype, count=1)]

3.3 带RGB颜色和各种相加的信息

• 如果要带RGB信息，这需要rviz中选择RGB作为color transform; 只在rviz2中测试，rviz不知道可不可以。
  

# 将xyz_fields 换成xyzrgb_fields，并且保证points(N,6）
xyzrgb_fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
                   PointField(name='y', offset=4, datatype=dtype, count=1),
                   PointField(name='z', offset=8, datatype=dtype, count=1),
                   PointField(name='r', offset=12, datatype=dtype, count=1),
                   PointField(name='g', offset=16, datatype=dtype, count=1),
                   PointField(name='b', offset=20, datatype=dtype, count=1)]
# 当然也支持其他附带信息
# 感觉这个A透明度不太起作用，因为这个topic 本身就有Alpha表示透明度
xyzrgba_fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
                   PointField(name='y', offset=4, datatype=dtype, count=1),
                   PointField(name='z', offset=8, datatype=dtype, count=1),
                   PointField(name='r', offset=12, datatype=dtype, count=1),
                   PointField(name='g', offset=16, datatype=dtype, count=1),
                   PointField(name='b', offset=20, datatype=dtype, count=1),
                   PointField(name='a', offset=24, datatype=dtype, count=1)]
# x,y,z,r,g,b, cls_id, cls_score. 当使用selection功能的时候，可以查看当前点的预测类别和置信度。
xyzrgbcs_fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
                   PointField(name='y', offset=4, datatype=dtype, count=1),
                   PointField(name='z', offset=8, datatype=dtype, count=1),
                   PointField(name='r', offset=12, datatype=dtype, count=1),
                   PointField(name='g', offset=16, datatype=dtype, count=1),
                   PointField(name='b', offset=20, datatype=dtype, count=1),
                   PointField(name='cls_id', offset=24, datatype=dtype, count=1),
                   PointField(name='score', offset=24, datatype=dtype, count=1)]
             

4. 绘制image输出

from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class MyPublisher(Node):
    rate = 10
    moving = True

    header = Header()
    header.frame_id = 'my_frame_id'

    dtype = PointField.FLOAT32
    point_step =16


    def __init__(self, node_name='pc_publisher', interval=10, prefix=""):
        super().__init__(node_name)
        self.pubs_dict = dict()
        self.pubs_dict['myimage'] = self.create_publisher(Image, "myimage", 10)

        self.br = CvBridge()

		def publish_image(self, range_image, topic='myimage'):
        assert (len(range_image.shape) == 3 and range_image.shape[2] == 3) or \
            len(range_image.shape) == 2, "my image shape error"

        self.pubs_dict[topic].publish(self.br.cv2_to_imgmsg(range_image))


if __name__ == '__main__':
	  rclpy.init()
    pub = PointCloudPublisher("test_pub", interval=5)
    ########## test publish image type
    import time
    from PIL import Image as PILImage
    for i in range(30):
        image = PILImage.open('./{}.png'.format((i % 2) + 1)).convert('RGB')
        im_width, im_height = image.size
        image_numpy = np.array(image.getdata()).astype('float32') / 255.0
        # image_numpy = image_numpy.reshape(im_height, im_width, 3)[:,:,0]
        pub.publish_image(image_numpy)
        time.sleep(1)

    rclpy.shutdown()

参考文献

---

1. ros marker 绘制box
2. ros2 官方github代码示例