From e61735f86c098a9e949335a08d182e226d1b68b7 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 14 Mar 2023 15:58:54 +0100 Subject: [PATCH] Plot traj --- scripts/plot_json_objects_scenario.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py index ee51993..804275a 100755 --- a/scripts/plot_json_objects_scenario.py +++ b/scripts/plot_json_objects_scenario.py @@ -746,6 +746,31 @@ if __name__ == "__main__": #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1) #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1) #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1) + if ("traj" in scenario and isinstance(scenario["traj"], list) + and len(scenario["traj"]) > 0): + print(len(scenario["traj"])) + # print(scenario["traj"]) + print(len(plot_nodes(scenario["traj"])[0])) + # print(plot_nodes(scenario["traj"])) + plt.plot(*plot_nodes(scenario["traj"]), color="red") + plt.plot(*plot_car(scenario["traj"][-1]), color="red") + for p in scenario["traj"]: + if len(p) > 4: + if p[3] > 0: + plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+") + elif p[3] < 0: + plt.plot(p[0] - MINX, p[1] - MINY, color="green", marker="x") + else: + plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".") + else: + plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker=".") + #plt.plot(*plot_car(p), color="red") + pass + #cc = plot_car_corners(p) + #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1) + #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1) + #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1) + #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1) if "ispath" in scenario and len(scenario["ispath"]) > 0: plt.plot(*plot_nodes(scenario["ispath"]), color="green") for p in scenario["ispath"]: -- 2.39.2