from sys import argv, exit
import matplotlib
+BCAR_SD = 0
# j1
-#BCAR_W = 1.625
-#BCAR_SD = 0
-#BCAR_DF = 3.105
-#BCAR_DR = 0.655
+#BCAR_W = 1.771
+#BCAR_DF = 3.427
+#BCAR_DR = 0.657
# wang 2017
#BCAR_W = 1.81
-#BCAR_SD = 0
#BCAR_DF = 3.7
#BCAR_DR = 4.85 - 3.7
# jhang 2020
-BCAR_W = 2.022
-BCAR_SD = 0
-BCAR_DF = 4.236
-BCAR_DR = 5.171 - 4.236
+#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
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
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
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][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"]: