multi_path_tracking.py

from path_tracker import *

from guided_mrmp.planners.singlerobot.RRTStar import RRTStar


from guided_mrmp.utils import Conflict, Robot, Env, Plotting

def plot(x_histories, y_histories, h_histories, wp_paths):
    plt.style.use("ggplot")
    fig = plt.figure()
    plt.ion()
    plt.show()
    plt.clf()

    print(f"hist = {x_histories}")

    for x_history, y_history, h_history, path in zip(x_histories, y_histories, h_histories, wp_paths):
        print(x_history)
        plt.plot(
            path[0, :],
            path[1, :],
            c="tab:orange",
            marker=".",
            label="reference track",
        )

        plt.plot(
            x_history,
            y_history,
            c="tab:blue",
            marker=".",
            alpha=0.5,
            label="vehicle trajectory",
        )

        plot_roomba(x_history[-1], y_history[-1], h_history[-1])


        plt.ylabel("y")
        plt.yticks(
            np.arange(min(path[1, :]) - 1.0, max(path[1, :] + 1.0) + 1, 1.0)
        )
        plt.xlabel("x")
        plt.xticks(
            np.arange(min(path[0, :]) - 1.0, max(path[0, :] + 1.0) + 1, 1.0)
        )
    plt.axis("equal")
    

    plt.tight_layout()

    plt.draw()
    plt.pause(0.1)
    input()

if __name__ == "__main__":

    initial_pos_1 = np.array([0.0, 0.1, 0.0, 0.0])
    target_vocity = 3.0 # m/s
    T = 1  # Prediction Horizon [s]
    DT = 0.2  # discretization step [s]


    x_start = (0, 0)  # Starting node
    x_goal = (10, 3)  # Goal node

    env = Env([0,10], [0,10], [], [])

    rrtstar = RRTStar(env, x_start, x_goal, 0.5, 0.05, 500, r=2.0)
    rrtstarpath = rrtstar.run()
    rrtstarpath = list(reversed(rrtstarpath))
    xs = []
    ys = []
    for node in rrtstarpath:
        xs.append(node[0])
        ys.append(node[1])

    wp_1 = [xs,ys]
    sim = PathTracker(initial_position=initial_pos_1, target_v=target_vocity, T=T, DT=DT, waypoints=wp_1)
    x1,y1,h1 = sim.run(show_plots=False)
    path1 = sim.path
    
    initial_pos_2 = np.array([10.0, 5.1, 0.0, 0.0])
    target_vocity = 3.0 # m/s


    x_start = (10, 5)  # Starting node
    x_goal = (1, 1)  # Goal node
    rrtstar = RRTStar(env,x_start, x_goal, 0.5, 0.05, 500, r=2.0)
    rrtstarpath = rrtstar.run()
    rrtstarpath = list(reversed(rrtstarpath))
    xs = []
    ys = []
    for node in rrtstarpath:
        xs.append(node[0])
        ys.append(node[1])

    wp_2 = [xs,ys]

    
    sim2 = PathTracker(initial_position=initial_pos_2, target_v=target_vocity, T=T, DT=DT, waypoints=wp_2)
    x2,y2,h2 = sim2.run(show_plots=False)
    path2 = sim2.path

    # for 

    plot([x1,x2], [y1,y2], [h1,h2], [path1, path2])