rx_plot_pose_with_map.py 文件源码

def visualize_jackal(figure, pose, img):
    pyplot.cla()
    fig = figure
    ax = fig.add_subplot(111)
    ax.imshow(img, cmap=cm.Greys_r)
    ax.axis('image')
    if pose:
        meter_to_pixel = 10.6
        xl = pose[0]*meter_to_pixel
        yl = pose[1]*meter_to_pixel
        dl = pose[2]
        ax.plot(xl, yl, 'ro', markersize=10)
        L1 = 0.8*meter_to_pixel
        L2 = 1.6*meter_to_pixel
        car=[(xl-L1,yl-L1), (xl-L1,yl+ L1), (xl, yl+L2), (xl+L1, yl+L1), (xl+L1,yl-L1)]
        polygon2 = Polygon(transform(car, [xl,yl],dl+3.14), fill = True, facecolor='blue', edgecolor='blue', lw=4, zorder=2)
        ax.add_patch(polygon2)    
    ax.grid()
    #fig.subplots_adjust(0.003,0.062,0.97,0.94)
    #pyplot.show()
    pyplot.pause(0.01)
    return fig


#==============================
#==============================