+#!/usr/bin/env python3
"""Plot JSON formatted scenario."""
from json import loads
-from math import cos, pi, sin
+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_MTR = 10.820
-BCAR_WB = 2.450
-BCAR_W = 1.625
-BCAR_L = 3.760
BCAR_SD = 0
-BCAR_DF = 3.105
-BCAR_DR = 0.655
+# 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
# - 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"] = 12
- plt.rcParams['hatch.linewidth'] = 0.5
- plt.rcParams['lines.linewidth'] = 1.0
+ plt.rcParams["font.size"] = 24
+ plt.rcParams['hatch.linewidth'] = 6.0
+ plt.rcParams['lines.linewidth'] = 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_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 Scenario 4-0 detail use:
# - font size 22
#ax.set_title("Scenario 2")
- #ax.set_xlim([32, 51]) # w=19
- #ax.set_ylim([1, 19]) # h=18
+ #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, 47]) # w=15
+ #ax.set_xlim([32.5, 48.5]) # w=16
#ax.set_ylim([1, 16]) # h=15
# For Scenario 5-1 detail use:
# For Scenario 4-1-{0,14} detail use:
# - font size 22
#ax.set_title("Scenario 5") # Scenario 6
- #ax.set_xlim([31, 47.5]) # w=16.5
+ #ax.set_xlim([32.5, 48.5]) # w=16
#ax.set_ylim([1, 16.3]) # h=15.3
- # For Scenario 5-3-{35,} detail use:
+ # 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([8.1, 28])
+ #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.text(35.2, 5.5, "Goal configuration", color="green")
# For scenario 5-3
- #ax.set_title("Guessed goal")
+ #ax.set_title("Computed goal")
#ax.set_xlim([6.8, 16.2])
#ax.set_ylim([15, 20])
# Set min and max to center the plot.
MINX = scenario["init"][0]
MINY = scenario["init"][1]
- if "obst" in scenario and len(scenario["obst"]) > 0:
+ 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
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(
)
#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(
lw=0,
)
# Plot obstacles, slot.
- if "obst" in scenario and len(scenario["obst"]) > 0:
+ 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="//")
+ 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(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
inits = "inits"
inits_c = "orange"
- if False and inits in scenario:
+ if True and inits in scenario:
max_i = len(scenario[inits]) - 1
ii = 0
i = scenario[inits][ii]
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:
scenario["init"][1] - MINY,
color="red",
marker="+",
- ms=12
+ markeredgewidth=2,
+ ms=24
)
- 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 "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"][1] - MINY,
color="magenta",
marker="+",
- ms=12
- )
- 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 and len(scenario["goal"]) == 3:
- plt.plot(*plot_car(scenario["goal"]), color="green")
- plt.plot(
- scenario["goal"][0] - MINX,
- scenario["goal"][1] - MINY,
- color="green",
- marker="+",
- ms=12
+ 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 "orig_path" in sc2 and len(sc2["orig_path"]) > 0:
- plt.plot(
- *plot_nodes(sc2["orig_path"]),
- color="orange",
- linewidth=1,
- linestyle="dotted",
- )
- if sc2 and "path" in sc2 and len(sc2["path"]) > 0:
+ 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 "orig_path" in scenario and len(scenario["orig_path"]) > 0:
+ if ("opath" in scenario and isinstance(scenario["opath"], list)
+ and len(scenario["opath"]) > 0):
plt.plot(
- *plot_nodes(scenario["orig_path"]),
+ *plot_nodes(scenario["opath"]),
color="blue",
linewidth=1,
linestyle="dotted",
)
- if "path" in scenario and len(scenario["path"]) > 0:
- plt.plot(
- *plot_nodes(scenario["path"]),
- color="blue",
- linewidth=1,
- )
+ 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 ("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)
projects / hubacji1 / iamcar2.git / blobdiff