--- /dev/null
+#!/usr/bin/env python3
+"""Plot JSON formatted scenario."""
+from json import loads
+from math import cos, pi, sin, atan, atan2
+from math import inf
+from matplotlib import pyplot as plt
+from sys import argv, exit
+import matplotlib
+
+BCAR_SD = 0
+# j1
+#BCAR_W = 1.771
+#BCAR_DF = 3.427
+#BCAR_DR = 0.657
+# wang 2017
+#BCAR_W = 1.81
+#BCAR_DF = 3.7
+#BCAR_DR = 4.85 - 3.7
+# jhang 2020
+#BCAR_W = 2.022
+#BCAR_DF = 4.236
+#BCAR_DR = 5.171 - 4.236
+# Opel Corsa
+#BCAR_W = 1.532
+#BCAR_DF = 3.212
+#BCAR_DR = 3.622 - BCAR_DF
+# Porsche Cayenne
+BCAR_W = 1.983
+BCAR_DF = 2.895 + 0.9
+BCAR_DR = 1.123
+
+MINX = inf
+MINY = inf
+
+def get_scenario(fname):
+ """Load scenario from file."""
+ if fname is None:
+ raise ValueError("File name as argument needed")
+ with open(fname, "r") as f:
+ scenario = loads(f.read())
+ return scenario
+
+def plot_nodes(nodes=[]):
+ """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
+
+ Keyword arguments:
+ nodes -- The list of nodes to plot.
+ """
+ xcoords = []
+ ycoords = []
+ for n in nodes:
+ xcoords.append(n[0] - MINX)
+ ycoords.append(n[1] - MINY)
+ return (xcoords, ycoords)
+
+def plot_car(pose):
+ """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
+
+ Keyword arguments:
+ pose -- The pose of a car.
+ """
+ lfx = pose[0]
+ lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lfx += BCAR_DF * cos(pose[2])
+ lfx += BCAR_SD * cos(pose[2])
+
+ lf3x = pose[0]
+ lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lf3x += 2/3 * BCAR_DF * cos(pose[2])
+ lf3x += BCAR_SD * cos(pose[2])
+
+ lrx = pose[0]
+ lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lrx += -BCAR_DR * cos(pose[2])
+ lrx += -BCAR_SD * cos(pose[2])
+
+ rrx = pose[0]
+ rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rrx += -BCAR_DR * cos(pose[2])
+ rrx += -BCAR_SD * cos(pose[2])
+
+ rfx = pose[0]
+ rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rfx += BCAR_DF * cos(pose[2])
+ rfx += BCAR_SD * cos(pose[2])
+
+ rf3x = pose[0]
+ rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rf3x += 2/3 * BCAR_DF * cos(pose[2])
+ rf3x += BCAR_SD * cos(pose[2])
+
+ lfy = pose[1]
+ lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lfy += BCAR_DF * sin(pose[2])
+ lfy += BCAR_SD * sin(pose[2])
+
+ lf3y = pose[1]
+ lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lf3y += 2/3 * BCAR_DF * sin(pose[2])
+ lf3y += BCAR_SD * sin(pose[2])
+
+ lry = pose[1]
+ lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lry += -BCAR_DR * sin(pose[2])
+ lry += -BCAR_SD * sin(pose[2])
+
+ rry = pose[1]
+ rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rry += -BCAR_DR * sin(pose[2])
+ rry += -BCAR_SD * sin(pose[2])
+
+ rfy = pose[1]
+ rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rfy += BCAR_DF * sin(pose[2])
+ rfy += BCAR_SD * sin(pose[2])
+
+ rf3y = pose[1]
+ rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rf3y += 2/3 * BCAR_DF * sin(pose[2])
+ rf3y += BCAR_SD * sin(pose[2])
+
+ cfx = pose[0]
+ cfx += BCAR_DF * cos(pose[2])
+ cfx += BCAR_SD * cos(pose[2])
+
+ cfy = pose[1]
+ cfy += BCAR_DF * sin(pose[2])
+ cfy += BCAR_SD * sin(pose[2])
+
+ xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
+ ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
+ return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
+
+def plot_car_corners(pose):
+ """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
+
+ Keyword arguments:
+ pose -- The pose of a car.
+ """
+ lfx = pose[0]
+ lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lfx += BCAR_DF * cos(pose[2])
+ lfx += BCAR_SD * cos(pose[2])
+
+ lrx = pose[0]
+ lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lrx += -BCAR_DR * cos(pose[2])
+ lrx += -BCAR_SD * cos(pose[2])
+
+ rrx = pose[0]
+ rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rrx += -BCAR_DR * cos(pose[2])
+ rrx += -BCAR_SD * cos(pose[2])
+
+ rfx = pose[0]
+ rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rfx += BCAR_DF * cos(pose[2])
+ rfx += BCAR_SD * cos(pose[2])
+
+ lfy = pose[1]
+ lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lfy += BCAR_DF * sin(pose[2])
+ lfy += BCAR_SD * sin(pose[2])
+
+ lry = pose[1]
+ lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lry += -BCAR_DR * sin(pose[2])
+ lry += -BCAR_SD * sin(pose[2])
+
+ rry = pose[1]
+ rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rry += -BCAR_DR * sin(pose[2])
+ rry += -BCAR_SD * sin(pose[2])
+
+ rfy = pose[1]
+ rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rfy += BCAR_DF * sin(pose[2])
+ rfy += BCAR_SD * sin(pose[2])
+
+ xcoords = (lfx, lrx, rrx, rfx)
+ ycoords = (lfy, lry, rry, rfy)
+ return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
+
+if __name__ == "__main__":
+ sc2 = None
+ if (len(argv) == 2):
+ SCEN_FILE = argv[1]
+ elif (len(argv) == 3):
+ SCEN_FILE = argv[1]
+ SCEN_FILE2 = argv[2]
+ sc2 = get_scenario(SCEN_FILE2)
+ else:
+ SCEN_FILE = "sc.json"
+
+ scenario = get_scenario(SCEN_FILE)
+
+ # Font size to be approximately the same in the paper:
+ # - sc1-0: 16
+ # - sc3-2, Intro: 12
+ # - sc4-0+: 22
+ plt.rc('axes', unicode_minus=False)
+ plt.rcParams["figure.figsize"] = [14, 7]
+ plt.rcParams["font.family"] = "cmr10"
+ plt.rcParams["font.size"] = 24
+ plt.rcParams['hatch.linewidth'] = 0.5 # 6.0
+ plt.rcParams['lines.linewidth'] = 1.0 # 2.0
+ fig = plt.figure()
+
+ # here subplot starts
+ ax = fig.add_subplot(111)
+ ax.set_aspect("equal")
+ #ax.set_title("Real-world parking scenario")
+ ax.set_xlabel("x [m]")
+ ax.set_ylabel("y [m]")
+ # For stage, comment upper, uncomment following:
+ #plt.xticks([])
+ #plt.yticks([])
+
+ # sps 99
+ #ax.set_xlim([4, 32]) # 28
+ #ax.set_ylim([7, 29]) # 22
+ # sps 551
+ #ax.set_xlim([-1, 27]) # 28
+ #ax.set_ylim([-1, 21]) # 22
+ # sps 591
+ #ax.set_xlim([-9.9, 18.1]) # 28
+ #ax.set_ylim([-4.9, 17.1]) # 22
+ # sps 783
+ #ax.set_xlim([4, 32]) # 28
+ #ax.set_ylim([-4, 18]) # 22
+ # sps 510
+ #ax.set_xlim([-7, 21]) # 28
+ #ax.set_ylim([7, 29]) # 22
+ # sps 501
+ #ax.set_xlim([-7, 21]) # 28
+ #ax.set_ylim([2, 24]) # 22
+ # sps 167
+ #ax.set_xlim([-1, 27]) # 28
+ #ax.set_ylim([-1, 21]) # 22
+
+ # For Possible Entry Points (Possible Entry Configurations) use:
+ #plt.rcParams["font.size"] = 26
+ #ax.set_xlim([37.6, 45.6])
+ #ax.set_ylim([2.4, 8.5])
+ #ax.set_title("Possible configurations")
+
+ # For Last Maneuver use:
+ #ax.set_xlim([38, 44])
+ #ax.set_ylim([3.1, 6.9])
+
+ # For Scenario 3-2 detail use:
+ # - font size 22
+ #ax.set_title("Scenario 1")
+ #ax.set_xlim([1, 16]) # w=15
+ #ax.set_ylim([35.1, 49.9]) # h=15
+
+ # For Scenario 4-0 detail use:
+ # - font size 22
+ #ax.set_title("Scenario 2")
+ #ax.set_xlim([32, 53]) # w=19
+ #ax.set_ylim([0.5, 19.5]) # h=18
+
+ # For Scenario 4-1 detail use:
+ # - font size 22
+ #ax.set_title("Scenario 3")
+ #ax.set_xlim([32.5, 48.5]) # w=16
+ #ax.set_ylim([1, 16]) # h=15
+
+ # For Scenario 5-1 detail use:
+ # - font size 22
+ #ax.set_title("Scenario 4")
+ #ax.set_xlim([10.1, 27])
+ #ax.set_ylim([10.1, 26])
+
+ # For Scenario 4-1-{0,14} detail use:
+ # - font size 22
+ #ax.set_title("Scenario 5") # Scenario 6
+ #ax.set_xlim([32.5, 48.5]) # w=16
+ #ax.set_ylim([1, 16.3]) # h=15.3
+
+ # For Scenario 5-3-34 detail use:
+ # - font size 22
+ #ax.set_title("Scenario 7") # Scenario 8
+ #ax.set_xlim([5.5, 27.5])
+ #ax.set_ylim([15.1, 35])
+
+ # For Scenario 5-3-29 detail use:
+ # - font size 22
+ #ax.set_title("Scenario 8")
+ #ax.set_xlim([5.5, 27.5])
+ #ax.set_ylim([10.1, 30])
+
+ # For Real-world parking scenario in Introduction section use:
+ #ax.set_title("Real-world parking scenario with artificial obstacle")
+ #ax.set_xlim([23, 58.5])
+ #ax.set_ylim([-8.4, 20.9])
+ #ax.text(34, 9.2, "Initial configuration", color="red")
+ #ax.text(43.5, 8.5, "Final path", color="blue")
+ #ax.text(48.25, 5.5, "Entry\nconfigurations", color="orange", ha="right")
+ #ax.text(38, 3.8, "Parking\nslot", color="blue", ha="right", backgroundcolor="white")
+ #ax.text(35.2, 5.5, "Goal configuration", color="green")
+
+ # For scenario 5-3
+ #ax.set_title("Computed goal")
+ #ax.set_xlim([6.8, 16.2])
+ #ax.set_ylim([15, 20])
+
+ # For simple scenarios 92 and 96 (simple-1k-test49)
+ # - Use MINY=-25, MINX=-10 for 96.
+ #ax.set_title("Simple parking scenario")
+ #ax.set_xlim([-11, 24]) # w=35
+ #ax.set_ylim([-2.5, 37.5]) # h=40
+
+ # Set min and max to center the plot.
+ MINX = scenario["init"][0]
+ MINY = scenario["init"][1]
+ MAXX = scenario["init"][0]
+ MAXY = scenario["init"][1]
+ if ("obst" in scenario and isinstance(scenario["obst"], list)
+ and len(scenario["obst"]) > 0):
+ for o in scenario["obst"]:
+ if not o:
+ continue
+ for n in o:
+ if n[0] < MINX:
+ MINX = n[0]
+ if n[1] < MINY:
+ MINY = n[1]
+ if n[0] > MAXX:
+ MAXX = n[0]
+ if n[1] > MAXY:
+ MAXY = n[1]
+ print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
+ #MINY = -25
+ #MINX = -10
+ c1 = plt.Circle(
+ (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
+ 4.8677125017335845,
+ color='red',
+ fill=False,
+ )
+ #ax.add_patch(c1)
+ c2 = plt.Circle(
+ (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
+ 3.1984427539075178,
+ color='red',
+ fill=False,
+ )
+ #ax.add_patch(c2)
+ c3 = plt.Circle(
+ (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
+ 5.736429638720212,
+ color='red',
+ fill=False,
+ )
+ #ax.add_patch(c3)
+ c4 = plt.Circle(
+ (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
+ 3.1984427539075178,
+ color='red',
+ fill=False,
+ )
+ #ax.add_patch(c4)
+
+ # For Goal Zone figure, "Goal zone" file name in j1/figs/
+ def r2d(w):
+ return w*180.0/pi
+ # ax.set_ylim([-4.8, 2.8])
+ # ax.set_xlim([-13, 5])
+ gz_ccr = matplotlib.patches.Arc(
+ (-744206.185356 - MINX, -1044330.294266 - MINY),
+ 5.207071 * 2, 5.207071 * 2,
+ theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
+ theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
+ color="magenta",
+ fill=False,
+ lw=2,
+ )
+ # ax.add_patch(gz_ccr)
+ gz_ccr = matplotlib.patches.Arc(
+ (-744206.185356 - MINX + 3.99, -1044330.294266 - MINY + 2.05),
+ 5.207071 * 2, 5.207071 * 2,
+ theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
+ theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
+ color="magenta",
+ fill=False,
+ lw=2, ls="dotted",
+ )
+ # ax.add_patch(gz_ccr)
+ gz_gh = 0.47424360277825361
+ gz_ih = -0.27424360277825361
+ def li(x, y, h, le=10.0):
+ return (x, x + le * cos(h)), (y, y + le * sin(h))
+ # gz border
+ # plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_gh),
+ # color="orange", ls="dotted")
+ # plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_ih),
+ # color="red", ls="dotted")
+ # path
+ # plt.plot(
+ # *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
+ # color="orange", ls="solid")
+ # plt.plot(
+ # *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
+ # color="red", ls="solid")
+ # ax.text(
+ # -744208.5632466434 - MINX,
+ # -1044325.6618554679 - MINY - 1.5,
+ # "C",
+ # color="orange",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # )
+ # ax.text(
+ # -744208.5632466434 - MINX + 0.35,
+ # -1044325.6618554679 - MINY - 1.7,
+ # "E",
+ # color="orange",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # fontsize=16,
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX,
+ # -1044323.6618554679 - MINY - 1.5,
+ # "C",
+ # color="red",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX + 0.35,
+ # -1044323.6618554679 - MINY - 1.7,
+ # "g",
+ # color="red",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # fontsize=16,
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX,
+ # -1044323.6618554679 - MINY - 3.9,
+ # "θ",
+ # color="red",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX + 0.35,
+ # -1044323.6618554679 - MINY - 4.1,
+ # "G",
+ # color="red",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # fontsize=16,
+ # )
+ # ax.arrow(
+ # -744199.2632466434 - MINX,
+ # -1044323.6618554679 - MINY - 3.18,
+ # cos(gz_ih),
+ # sin(gz_ih),
+ # width=0.05,
+ # color="red",
+ # zorder=2,
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX,
+ # -1044323.6618554679 - MINY + 1.9,
+ # "θ",
+ # color="orange",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX + 0.35,
+ # -1044323.6618554679 - MINY + 1.7,
+ # "E",
+ # color="orange",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ # fontsize=16,
+ # )
+ # ax.arrow(
+ # -744199.2632466434 - MINX,
+ # -1044323.6618554679 - MINY + 1.22,
+ # cos(gz_gh),
+ # sin(gz_gh),
+ # width=0.05,
+ # color="orange",
+ # zorder=2,
+ # )
+ # ax.text(
+ # -744199.2632466434 - MINX + 2,
+ # -1044323.6618554679 - MINY + -3,
+ # "G",
+ # color="dimgray",
+ # fontfamily="serif",
+ # fontstyle="normal",
+ # fontweight="bold",
+ # backgroundcolor="white",
+ # )
+ # ax.fill((
+ # -MINX -744204.775115,
+ # -MINX -744204.775115 + 15 * cos(0.47424360277825361),
+ # -MINX -744204.775115 + 15 * cos(0.27424360277825361),
+ # -MINX -744204.775115,
+ # ), (
+ # -MINY -1044325.281765,
+ # -MINY -1044325.281765 + 15 * sin(0.47424360277825361),
+ # -MINY -1044325.281765 - 15 * sin(0.27424360277825361),
+ # -MINY -1044325.281765,
+ # ), color="gainsboro", fill=False, hatch="x")
+ # # --- End of Goal Zone figure ---
+
+ # Plot all the nodes (if exists.)
+ if "nodes_x" in scenario and "nodes_y" in scenario:
+ plt.plot(
+ [x - MINX for x in scenario["nodes_x"]],
+ [y - MINY for y in scenario["nodes_y"]],
+ color="lightgray",
+ marker="o",
+ ms=2,
+ lw=0,
+ )
+ # Plot all the steered2 nodes (if exists.)
+ if "steered2_x" in scenario and "steered2_y" in scenario:
+ plt.plot(
+ [x - MINX for x in scenario["steered2_x"]],
+ [y - MINY for y in scenario["steered2_y"]],
+ color="orange",
+ marker="o",
+ ms=2,
+ lw=0,
+ )
+ # Plot all the steered1 nodes (if exists.)
+ if "steered1_x" in scenario and "steered1_y" in scenario:
+ plt.plot(
+ [x - MINX for x in scenario["steered1_x"]],
+ [y - MINY for y in scenario["steered1_y"]],
+ color="blue",
+ marker="o",
+ ms=2,
+ lw=0,
+ )
+ # Plot obstacles, slot.
+ if ("obst" in scenario and isinstance(scenario["obst"], list)
+ and len(scenario["obst"]) > 0):
+ for o in scenario["obst"]:
+ if not o:
+ continue
+ ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
+ if "slot" in scenario and len(scenario["slot"]) > 0:
+ plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
+ #for s in scenario["slot"]:
+ # plt.plot(*plot_nodes(s), color="black")
+
+ # For the Possible Entry Configurations from the paper, use:
+ #ax.set_title("Computed configurations")
+ inits = "insides"
+ inits_c = "green"
+ if False and inits in scenario:
+ max_i = len(scenario[inits]) - 1
+ ii = 0
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = int(max_i / 4)
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = int(max_i / 2)
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = int(max_i * 3/4)
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = max_i
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ inits = "inits"
+ inits_c = "orange"
+ if True and inits in scenario:
+ max_i = len(scenario[inits]) - 1
+ ii = 0
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = int(max_i / 4)
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = int(max_i / 2)
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = int(max_i * 3/4)
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ ii = max_i
+ i = scenario[inits][ii]
+ plt.plot(*plot_car(i), color=inits_c)
+ plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+ # Possible/Candidate entries
+ #ax.text(
+ # 44.95,
+ # 5.5,
+ # "p",
+ # color="blue",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ #)
+ #ax.arrow(
+ # scenario["slot"][-1][0] - MINX,
+ # scenario["slot"][-1][1] - MINY,
+ # -2,
+ # 0,
+ # width=0.05,
+ # color="blue",
+ # zorder=2,
+ #)
+ #ax.text(
+ # scenario["slot"][-1][0] - MINX - 2,
+ # 5.5,
+ # "δ",
+ # color="blue",
+ # fontfamily="serif",
+ # fontstyle="italic",
+ #)
+
+ # Plot `init`, `entry`, and `goal` configurations.
+ if "init" in scenario and len(scenario["init"]) == 3:
+ plt.plot(*plot_car(scenario["init"]), color="red")
+ plt.plot(
+ scenario["init"][0] - MINX,
+ scenario["init"][1] - MINY,
+ color="red",
+ marker="+",
+ markeredgewidth=2,
+ ms=24
+ )
+ #if "init" in scenario and len(scenario["init"]) == 4:
+ # plt.plot(*plot_car(scenario["init"]), color="red")
+ # scenario["init"][2] = scenario["init"][3]
+ # plt.plot(*plot_car(scenario["init"]), color="red")
+ # plt.plot(
+ # scenario["init"][0] - MINX,
+ # scenario["init"][1] - MINY,
+ # color="red",
+ # marker="+",
+ # ms=12
+ # )
+ #if "entries" in scenario:
+ # for e in scenario["entries"]:
+ # plt.plot(*plot_car(e), color="orange")
+ # plt.plot(
+ # e[0] - MINX,
+ # e[1] - MINY,
+ # color="orange",
+ # marker="+",
+ # ms=12
+ # )
+ if "entry" in scenario and len(scenario["entry"]) == 3:
+ plt.plot(*plot_car(scenario["entry"]), color="magenta")
+ plt.plot(
+ scenario["entry"][0] - MINX,
+ scenario["entry"][1] - MINY,
+ color="magenta",
+ marker="+",
+ markeredgewidth=2,
+ ms=24
+ )
+ if "entry" in scenario and len(scenario["entry"]) == 4:
+ esc = scenario["entry"]
+ plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
+ plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
+ plt.plot(
+ scenario["entry"][0] - MINX,
+ scenario["entry"][1] - MINY,
+ color="magenta",
+ marker="+",
+ ms=12
+ )
+ if "goal" in scenario:
+ if len(scenario["goal"]) == 3:
+ #plt.plot(*plot_car(scenario["goal"]), color="green")
+ plt.plot(*plot_car(scenario["goal"]), color="orange")
+ plt.plot(
+ scenario["goal"][0] - MINX,
+ scenario["goal"][1] - MINY,
+ #color="green",
+ color="orange",
+ marker="+",
+ markeredgewidth=2,
+ ms=24
+ )
+ # elif len(scenario["goal"]) == 4:
+ # ctp = scenario["goal"]
+ # plt.plot(*plot_car(scenario["goal"]), color="green")
+ # ctp[2] = ctp[3]
+ # plt.plot(*plot_car(scenario["goal"]), color="green")
+ # plt.plot(
+ # scenario["goal"][0] - MINX,
+ # scenario["goal"][1] - MINY,
+ # color="green",
+ # marker="+",
+ # ms=12
+ # )
+
+ # Plot `path` and `max_path`.
+ if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
+ and len(sc2["opath"]) > 0):
+ plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
+ if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
+ and len(sc2["path"]) > 0):
+ plt.plot(*plot_nodes(sc2["path"]), color="orange")
+ if ("opath" in scenario and isinstance(scenario["opath"], list)
+ and len(scenario["opath"]) > 0):
+ plt.plot(
+ *plot_nodes(scenario["opath"]),
+ color="blue",
+ linewidth=1,
+ linestyle="dotted",
+ )
+ if ("path" in scenario and isinstance(scenario["path"], list)
+ and len(scenario["path"]) > 0):
+ plt.plot(*plot_nodes(scenario["path"]), color="blue")
+ i = 0
+ for p in scenario["path"]:
+ if False and len(p) > 4:
+ if p[4] > 0:
+ plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
+ elif p[4] < 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="blue", marker="+")
+ #plt.plot(*plot_car(p), color="blue")
+ 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 i in [31]:
+ # plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
+ # pass
+ # i += 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"]:
+ #plt.plot(*plot_car(p), color="green")
+ 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 there are possible starts specified, you may print and plot them.
+ #if "starts" in scenario and len(scenario["starts"]) > 0:
+ # print("possible starts:")
+ # for p in scenario["starts"]:
+ # plt.plot(*p, color="red", marker="+", ms=12)
+ # print(" {}".format(p))
+
+ # For the Last Maneuver figure from the paper, use:
+ # - `init2` -- orange
+ #plt.plot(*plot_car(scenario["init2"]), color="orange")
+ #plt.plot(
+ # scenario["init2"][0] - MINX,
+ # scenario["init2"][1] - MINY,
+ # color="orange",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `goal2` -- orange
+ #plt.plot(*plot_car(scenario["goal2"]), color="orange")
+ #plt.plot(
+ # scenario["goal2"][0] - MINX,
+ # scenario["goal2"][1] - MINY,
+ # color="orange",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `goal2` -- middle (orange)
+ #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
+ #plt.plot(
+ # scenario["goal2"][0] - MINX,
+ # scenario["goal2"][1] - MINY,
+ # color="orange",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `init1` -- green
+ #plt.plot(*plot_car(scenario["init1"]), color="green")
+ #plt.plot(
+ # scenario["init1"][0] - MINX,
+ # scenario["init1"][1] - MINY,
+ # color="green",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `goal1` -- green
+ #plt.plot(*plot_car(scenario["goal1"]), color="green")
+ #plt.plot(
+ # scenario["goal1"][0] - MINX,
+ # scenario["goal1"][1] - MINY,
+ # color="green",
+ # #marker="+",
+ # ms=12
+ #)
+
+ # The `scenario` may also include:
+ # - `last` -- not sure what this is, see the source code. Maybe overlaps
+ # with the `goal`.
+ # - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
+ # Planner (PSP.))
+ # - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
+ # Planner (PSP.))
+ # - `max_orig_path` -- maximum original path. I used this when comparing
+ # original paths but I had to copy the `max_orig_path` by hand from
+ # different scenario result.
+ # - `orig_path` -- the path before the optimization.
+ # - `max_path` -- the maximum path after optimization. Must be copied by
+ # hand.
+ # - `path` -- optimized path of the scenario.
+
+ handles, labels = ax.get_legend_handles_labels()
+
+ # Uncommnent the following line and comment the plt.show() to store to the
+ # file.
+ #plt.savefig("out.pdf", bbox_inches="tight")
+ plt.show()
+ plt.close(fig)
projects / hubacji1 / path-to-traj.git / commitdiff