multi_path_tracking.py

    initial_pos_2 = np.array([6.0, 8.0, 4.8])
    target_vocity = 3.0 # m/s


    x_start = (6, 8)  # Starting node
    x_goal = (6.5, 2)  # Goal node
    rrtstar2 = RRTStar(env,x_start, x_goal, 0.5, 0.05, 500, r=2.0)
    rrtstarpath2,tree = rrtstar2.run()
    rrtstarpath2 = list(reversed(rrtstarpath2))
    xs = []
    ys = []
    for node in rrtstarpath2:
        xs.append(node[0])
        ys.append(node[1])

    wp_2 = [xs,ys]

    lib_2x3, lib_3x3, lib_2x5 = initialize_libraries()    

    sim = MultiPathTrackerDatabase(env, [initial_pos_1, initial_pos_2], dynamics, target_vocity, T, DT, [wp_1, wp_2], settings, lib_2x3, lib_3x3, lib_2x5)
    xs, ys, hs = sim.run(show_plots=False)
    paths = sim.paths

    print(f"path length here = {len(paths)}")

    # 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()