icnt_list = []
bcnt_list = []
rcnt_list = []
+ ncnt_list = []
+ cc_list = []
time_list = []
otime_list = []
min_s = 0
if s["goal_cc"] > max_c:
max_c = s["goal_cc"]
max_s = s["subdn"]
+ if s["goal_cc"] > 0:
+ cc_list.append(s["goal_cc"])
try:
icnt_list.append(s["icnt"])
bcnt_list.append(s["bcnt"])
rcnt_list.append(s["rcnt"])
+ ncnt_list.append(s["ncnt"])
except:
pass
try:
except:
pass
o_err_hist = list(err_hist)
- err_hist = [i * 100.0 / RUNS for i in o_err_hist]
+ err_hist = [i * 100.0 / RUNS if i > 0 else -6 for i in o_err_hist]
+ # this is failure rate
+ w_it = False
+ if w_it:
+ print("Failure {} % in the iteration {}".format(err_hist[w_it], w_it))
+ else:
+ try:
+ print("Failure 0.0 % in the iteration {}".format(err_hist.index(0.0)))
+ except:
+ print("Failure 0.0 % not reached")
+ # --- exit(1)
print("{} {}".format(min_s, max_s))
icnt_avg = np.average(icnt_list)
bcnt_avg = np.average(bcnt_list)
rcnt_avg = np.average(rcnt_list)
+ ncnt_avg = np.average(ncnt_list)
+ cc_avg = np.average(cc_list)
+ try:
+ cc_max = np.max(cc_list)
+ except:
+ cc_max = 0
+ time_avg = np.average(time_list)
+ time_max = np.max(time_list)
+ otime_avg = np.average(otime_list)
+ try:
+ otime_max = np.max(otime_list)
+ except:
+ otime_max = 0
br_avg = np.average([b / r for (b, r) in zip(bcnt_list, rcnt_list)])
print("avg. icnt = {}".format(icnt_avg))
print("avg. bcnt = {}".format(bcnt_avg))
print("avg. rcnt = {}".format(rcnt_avg))
+ print("avg. ncnt = {}".format(ncnt_avg))
+ print("avg. cc = {}".format(cc_avg))
+ print("max. cc = {}".format(cc_max))
+ print("avg. time = {}".format(time_avg))
+ print("max. time = {}".format(time_max))
+ print("avg. otime = {}".format(otime_avg))
+ print("max. otime = {}".format(otime_max))
print("b/r avg. {}".format(br_avg))
print("last avg. cost = {}".format(np.average(val_list[-1])))
- print("last max. cost = {}".format(perc(val_list[-1], 100)))
+ #print("last max. cost = {}".format(perc(val_list[-1], 100)))
+ #exit(1)
plt.rcParams["figure.figsize"] = [14, 7]
plt.rcParams["font.size"] = 24
f, ax = plt.subplots()
eax = ax.twinx()
- ax.set_title(scenarios[0]["fn"])
- ax.set_title("Scenario 3: Final path cost after optimization")
+ ax.grid(which="major", linestyle=":", color="tab:gray")
+ #ax.grid(which="minor", linestyle=":", color="gray")
+ #ax.set_title(scenarios[0]["fn"])
+ #ax.set_title("Scenario 3: Final path cost after optimization")
ax.set_xlabel("Number of iterations [-]")
ax.set_ylabel("Cost [m]")
eax.set_ylabel("Failure rate [%]")
#ax.set_ylim([20.1, 57]) # scenario 7
#ax.set_ylim([18, 43]) # scenario 2
ax.set_ylim([10, 60])
- eax.set_ylim([0, 100])
+ eax.set_ylim([-5, 100])
eax.fill_between(
x=range(L),
- y1=0,
+ y1=-5,
y2=[i for i in err_hist],
color="tab:red",
alpha=0.2,
ax.plot(
range(L),
[np.average(val_list[i]) for i in range(L)],
- color="tab:red",
+ color="tab:blue",
alpha=0.45,
label="Average cost",
)
#!/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
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:
)
#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(
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 / commitdiff