multi_path_tracking.py

    # plot(xs, ys, hs, paths, [rrtstar1.sampled_vertices, rrtstar2.sampled_vertices],2)

    # plot_sim(xs, ys, hs, paths)

def plot_roomba(x, y, yaw, color, fill, radius):
    """

    Args:
        x ():
        y ():
        yaw ():
    """
    fig = plt.gcf()
    ax = fig.gca()
    if fill: alpha = .3
    else: alpha = 1
    circle = plt.Circle((x, y), radius, color=color, fill=fill, alpha=alpha)
    ax.add_patch(circle)

    # Plot direction marker
    dx = 1 * np.cos(yaw)
    dy = 1 * np.sin(yaw)
    ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')



if __name__ == "__main__":
    main()