draw_utils.py 文件源码

def publish_pose_list(pub_ns, _poses, texts=[], stamp=None, size=0.05, frame_id='camera', seq=1):
    """
    Publish Pose List on:
    pub_channel: Channel on which the cloud will be published
    """
    poses = deepcopy(_poses)
    if not len(poses): return 
    arrs = np.vstack([pose.matrix[:3,:3].T.reshape((1,9)) for pose in poses]) * size
    arrX = np.vstack([pose.tvec.reshape((1,3)) for pose in poses])
    arrx, arry, arrz = arrs[:,0:3], arrs[:,3:6], arrs[:,6:9]

    # Point width, and height
    N = len(poses)

    markers = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)

    markers.header.frame_id = frame_id
    markers.header.stamp = stamp if stamp is not None else rospy.Time.now()
    markers.header.seq = seq
    markers.scale.x = size/20 # 0.01
    markers.scale.y = size/20 # 0.01

    markers.color.a = 1.0

    markers.pose.position = geom_msg.Point(0,0,0)
    markers.pose.orientation = geom_msg.Quaternion(0,0,0,1)

    markers.points = []
    markers.lifetime = rospy.Duration()        

    for j in range(N): 
        markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]), 
                                       geom_msg.Point(arrX[j,0] + arrx[j,0], 
                                                       arrX[j,1] + arrx[j,1], 
                                                       arrX[j,2] + arrx[j,2])])
        markers.colors.extend([std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0), std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)])

        markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]), 
                               geom_msg.Point(arrX[j,0] + arry[j,0], 
                                               arrX[j,1] + arry[j,1], 
                                               arrX[j,2] + arry[j,2])])
        markers.colors.extend([std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0), std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0)])


        markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]), 
                               geom_msg.Point(arrX[j,0] + arrz[j,0], 
                                               arrX[j,1] + arrz[j,1], 
                                               arrX[j,2] + arrz[j,2])])
        markers.colors.extend([std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0), std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)])

    _publish_marker(markers)