draw_utils.py 文件源码

def publish_octomap(pub_ns, arr, carr, size=0.1, stamp=None, frame_id='map', flip_rb=False): 
    """
    Publish cubes list:
    """
    marker = vis_msg.Marker(type=vis_msg.Marker.CUBE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)

    marker.header.frame_id = frame_id
    marker.header.stamp = stamp if stamp is not None else rospy.Time.now()

    # Point width, and height
    marker.scale.x = size
    marker.scale.y = size
    marker.scale.z = size

    N, D = arr.shape

    # XYZ
    inds, = np.where(~np.isnan(arr).any(axis=1))
    marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds]

    # RGB (optionally alpha)
    rax, bax = 0, 2
    # carr = carr.astype(np.float32) * 1.0 / 255
    if flip_rb: rax, bax = 2, 0
    if D == 3: 
        marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], 1.0) 
                         for j in inds]
    elif D == 4: 
        marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], carr[j,3])
                         for j in inds]

    marker.lifetime = rospy.Duration()
    _publish_marker(marker)