+#!/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"] = 22
- 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("Simple 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:
MINY = scenario["init"][1]
MAXX = scenario["init"][0]
MAXY = scenario["init"][1]
- 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.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
scenario["init"][1] - MINY,
color="red",
marker="+",
- ms=12
+ markeredgewidth=2,
+ ms=24
)
#if "init" in scenario and len(scenario["init"]) == 4:
# plt.plot(*plot_car(scenario["init"]), color="red")
# 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="+",
- # 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")
# )
if "goal" in scenario:
if len(scenario["goal"]) == 3:
- plt.plot(*plot_car(scenario["goal"]), color="green")
+ #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="green",
+ color="orange",
marker="+",
- ms=12
+ markeredgewidth=2,
+ ms=24
)
# elif len(scenario["goal"]) == 4:
# ctp = scenario["goal"]
projects / hubacji1 / iamcar2.git / blobdiff