rx_plot_pose.py 文件源码

def visualize_jackal(figure, pose):
    pyplot.cla()
    fig = figure
    ax = fig.add_subplot(111)
    if pose:
        xl = pose[0]
        yl = pose[1]
        dl = pose[2]
        ax.plot(xl, yl, 'ro', markersize=8)
        L1 = 0.4
        L2 = 0.8
        car=[(xl-L1,yl-L1), (xl-L1,yl+ L1), (xl, yl+L2), (xl+L1, yl+L1), (xl+L1,yl-L1)]
        Ecolor = 'Grey'            
        polygon2 = Polygon(transform(car, [xl,yl], dl), fill = True, facecolor=Ecolor, edgecolor='black', lw=4, zorder=2)
        ax.add_patch(polygon2)    
    ax.grid()
    ax.set_xlabel('x(m)')
    ax.set_ylabel('y(m)')
    ax.set_aspect('equal')
    ax.set_xlim(-10, 10)
    ax.set_ylim(-10, 10)
    #fig.subplots_adjust(0.003,0.062,0.97,0.94)
    #pyplot.show()
    pyplot.pause(0.01)
    return fig


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